From 7fbeabbc59482d7bfa467913dfa9e34f90ce0b4b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 17 Oct 2025 06:55:27 +0200 Subject: [PATCH 001/230] Add Jazzy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/thirdparty.repos | 6 ++-- .github/workflows/jazzy.yaml | 54 ++++++++++++++++++++++++++++++++++++ README.md | 5 ++-- 3 files changed, 59 insertions(+), 6 deletions(-) create mode 100644 .github/workflows/jazzy.yaml diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index c4abe867..6feffa6f 100644 --- a/.github/thirdparty.repos +++ b/.github/thirdparty.repos @@ -2,12 +2,12 @@ repositories: ThirdParty/EasyNavigation: type: git url: https://github.com/EasyNavigation/EasyNavigation.git - version: rolling + version: jazzy ThirdParty/yaets: type: git url: https://github.com/fmrico/yaets.git - version: rolling + version: jazzy ThirdParty/NavMap: type: git url: https://github.com/EasyNavigation/NavMap.git - version: rolling + version: jazzy diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml new file mode 100644 index 00000000..2b22efd5 --- /dev/null +++ b/.github/workflows/jazzy.yaml @@ -0,0 +1,54 @@ +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 + + - 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/README.md b/README.md index 314f141d..5c9b0802 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,9 @@ # EasyNav Plugins -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) -[![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - +[![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) ## Description **EasyNav Plugins** provides the official collection of plugins for the [Easy Navigation (EasyNav)](https://github.com/EasyNavigation) framework. From b22f43490fda22a2c5aa240f332f51bdc6e21019 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 17 Oct 2025 07:41:39 +0200 Subject: [PATCH 002/230] Fix yaets repo branch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/thirdparty.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index 6feffa6f..ec1e347a 100644 --- a/.github/thirdparty.repos +++ b/.github/thirdparty.repos @@ -6,7 +6,7 @@ repositories: ThirdParty/yaets: type: git url: https://github.com/fmrico/yaets.git - version: jazzy + version: jazzy-devel ThirdParty/NavMap: type: git url: https://github.com/EasyNavigation/NavMap.git From fcf197f5d6205e667af5a1ab267d8822a95437e9 Mon Sep 17 00:00:00 2001 From: estherag Date: Sat, 18 Oct 2025 11:25:07 +0200 Subject: [PATCH 003/230] easynav_simple_controller derivative and integrative logic, add check goal position orientation threshold, and add badges for tested controllers in jazzy --- 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 +++++++++++-- 7 files changed, 159 insertions(+), 77 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)); } From 806ebcd10f440f5ee350ea91eb685c7350cc700c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 21 Oct 2025 07:20:55 +0200 Subject: [PATCH 004/230] 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 | 3 ++- .github/workflows/kilted.yaml | 3 ++- .github/workflows/rolling.yaml | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 2b22efd5..33198952 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -19,7 +19,8 @@ jobs: steps: - name: Repo checkout uses: actions/checkout@v4 - + with: + ref: jazzy - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 with: 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 663e786b8cc176378771bd52ccc1de246721350d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 21 Oct 2025 07:25:35 +0200 Subject: [PATCH 005/230] 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 | 3 ++- .github/workflows/kilted.yaml | 3 ++- .github/workflows/rolling.yaml | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 2b22efd5..33198952 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -19,7 +19,8 @@ jobs: steps: - name: Repo checkout uses: actions/checkout@v4 - + with: + ref: jazzy - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 with: 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 439f91cebc2a096ef63ecfb897ef094e5c617fd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 25 Oct 2025 14:57:35 +0200 Subject: [PATCH 006/230] Astar for NavMap 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 | 49 +++---------------- 1 file changed, 7 insertions(+), 42 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 e1967352..650042e5 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(); @@ -170,32 +168,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 +184,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 +215,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 +240,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 4b84c166da00c315bd81bbf49079e62bfb35f520 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 28 Oct 2025 09:40:23 +0800 Subject: [PATCH 007/230] Path smoother 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.hpp | 28 ++++ .../easynav_navmap_planner/AStarPlanner.cpp | 146 ++++++++++++++++++ 2 files changed, 174 insertions(+) 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..c1210016 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. + */ + inline 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 650042e5..5a5b0c2c 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -133,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_); } @@ -140,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, From 8b87a9114c7af084ac324abb2074c25dd62063eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 28 Oct 2025 09:42:53 +0800 Subject: [PATCH 008/230] Path smoother MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../include/easynav_navmap_planner/AStarPlanner.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c1210016..09577ffc 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -104,7 +104,7 @@ class AStarPlanner : public PlannerMethodBase * to disable. Default: 0 (disabled). * @return nav_msgs::msg::Path Smoothed path, same frame_id and header stamp as input. */ - inline nav_msgs::msg::Path path_smoother( + nav_msgs::msg::Path path_smoother( const nav_msgs::msg::Path & in_path, const ::navmap::NavMap & navmap, int iterations = 5, 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 009/230] 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 010/230] 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 011/230] 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 012/230] 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 013/230] 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 014/230] 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 015/230] 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 016/230] 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 017/230] 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 018/230] 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 019/230] 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 020/230] 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 021/230] 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 022/230] 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 023/230] 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 024/230] 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 025/230] 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 026/230] 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 027/230] 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 028/230] 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 029/230] 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 030/230] 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 031/230] 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 032/230] 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 033/230] 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 034/230] 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 035/230] 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 036/230] 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 037/230] 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 038/230] 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 039/230] 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 040/230] 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 041/230] 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 042/230] 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 043/230] 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 044/230] 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 045/230] 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 046/230] 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 047/230] 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 048/230] 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 049/230] 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 050/230] 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 051/230] 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 052/230] 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 053/230] 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 054/230] 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 055/230] 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 056/230] 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 057/230] 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 058/230] 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 059/230] 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 060/230] 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 061/230] 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 062/230] 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 063/230] 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 064/230] 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 065/230] 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 066/230] 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 067/230] 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 068/230] 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 069/230] 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 070/230] 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 071/230] 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 072/230] 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 073/230] 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 074/230] 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 075/230] 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 076/230] 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 077/230] 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 078/230] 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 079/230] 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 080/230] 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 081/230] 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 082/230] 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 083/230] 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 084/230] 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 085/230] 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 086/230] 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 087/230] 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 088/230] 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 089/230] 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 090/230] 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 091/230] 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 092/230] 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 093/230] 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 094/230] 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 095/230] 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 096/230] 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 097/230] 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 098/230] 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 099/230] 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 100/230] 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 101/230] 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 102/230] 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 103/230] 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 5c5a7d905b529d5c0e8df88bb4a85b9ffdd8f49c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 3 Dec 2025 20:30:21 +0100 Subject: [PATCH 104/230] Sync CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/thirdparty.repos | 2 +- README.md | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index ec1e347a..6feffa6f 100644 --- a/.github/thirdparty.repos +++ b/.github/thirdparty.repos @@ -6,7 +6,7 @@ repositories: ThirdParty/yaets: type: git url: https://github.com/fmrico/yaets.git - version: jazzy-devel + version: jazzy ThirdParty/NavMap: type: git url: https://github.com/EasyNavigation/NavMap.git 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 b10a1e6c3d365dcfad5d6862b7babfa7fd2c134e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 3 Dec 2025 20:33:37 +0100 Subject: [PATCH 105/230] 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 | 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 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 106/230] 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 107/230] 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 108/230] 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 109/230] 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 110/230] 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 111/230] 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 112/230] 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 6c730d34d4358daff374696ac062b71316b271be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 4 Dec 2025 07:57:52 +0100 Subject: [PATCH 113/230] Sync to current EasyNavigation 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 | 2 +- .../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/InflationFilter.cpp | 1 + .../easynav_navmap_maps_manager/filters/ObstacleFilter.cpp | 2 +- .../easynav_octomap_maps_manager/filters/ObstacleFilter.cpp | 2 +- .../src/easynav_simple_maps_manager/SimpleMapsManager.cpp | 2 +- .../src/easynav_navmap_planner/AStarPlanner.cpp | 3 ++- 11 files changed, 16 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..5e5568b5 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -264,7 +264,7 @@ 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(); 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..48cd58c1 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -427,9 +427,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/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index 3c023944..4935d8bb 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,6 +41,7 @@ #include #include +#include #include "easynav_common/types/NavState.hpp" #include "easynav_common/types/Perceptions.hpp" 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..ffd0838f 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 @@ -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_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) { 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 94e9d86ea7389a8bd9620848c8fb2fb79b1ada5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 4 Dec 2025 08:03:41 +0100 Subject: [PATCH 114/230] Sync to current EasyNavigation 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 | 2 +- .../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/InflationFilter.cpp | 1 + .../easynav_navmap_maps_manager/filters/ObstacleFilter.cpp | 2 +- .../easynav_octomap_maps_manager/filters/ObstacleFilter.cpp | 2 +- .../src/easynav_simple_maps_manager/SimpleMapsManager.cpp | 2 +- 10 files changed, 14 insertions(+), 13 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..5e5568b5 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -264,7 +264,7 @@ 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(); 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..48cd58c1 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -427,9 +427,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/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index 3c023944..4935d8bb 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,6 +41,7 @@ #include #include +#include #include "easynav_common/types/NavState.hpp" #include "easynav_common/types/Perceptions.hpp" 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..ffd0838f 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 @@ -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_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 3492db69f58d7d44be17dd83c63160560622561a Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 4 Dec 2025 21:16:45 +0100 Subject: [PATCH 115/230] 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 116/230] 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 117/230] 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 118/230] 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 119/230] 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 120/230] 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 121/230] 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 122/230] 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 123/230] 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 124/230] 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 125/230] 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 126/230] 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 127/230] 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 128/230] 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 129/230] 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 130/230] 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 131/230] 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 132/230] 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 133/230] 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 134/230] 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 135/230] 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 136/230] 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 2dfc0513d66040015a65c22d7bc15894f243e279 Mon Sep 17 00:00:00 2001 From: migueldm Date: Tue, 20 Jan 2026 11:51:07 +0100 Subject: [PATCH 137/230] Added dual ukf for local and global localization --- .../config/example_params.yaml | 2 +- .../config/example_params_summit_dual.yaml | 200 ++++++++++++++++++ .../FusionLocalizer.hpp | 3 +- .../easynav_fusion_localizer/ukf_wrapper.hpp | 4 +- .../FusionLocalizer.cpp | 28 ++- .../easynav_fusion_localizer/ukf_wrapper.cpp | 22 +- 6 files changed, 241 insertions(+), 18 deletions(-) create mode 100644 localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml diff --git a/localizers/easynav_fusion_localizer/config/example_params.yaml b/localizers/easynav_fusion_localizer/config/example_params.yaml index 9c47f30b..905be4e5 100644 --- a/localizers/easynav_fusion_localizer/config/example_params.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params.yaml @@ -25,7 +25,7 @@ localizer_node: latitude_origin: 40.2834931 longitude_origin: -3.8207253999999997 altitude_origin: 746.184 - local_filter: + global_filter: two_d_mode: true publish_tf: true diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml new file mode 100644 index 00000000..3b0e1e43 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -0,0 +1,200 @@ +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: odom + + # # --- 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: + [true, true, true, + false, false, true, + true, true, true, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + imu0: /imu/data + imu0_config: + [false, false, false, + true, true, true, + false, false, false, + true, true, true, + false, false, false] + imu0_differential: true + 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] + global_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, true, + false, false, false, + true, true, true, + false, false, false] + imu0_differential: true + 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/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp index c29ce6e9..fcafdbb7 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -51,7 +51,8 @@ class FusionLocalizer : public easynav::LocalizerMethodBase void update(NavState & nav_state) override; private: - std::unique_ptr ukf_wrapper_; + std::unique_ptr ukf_local_{nullptr}; + std::unique_ptr ukf_global_{nullptr}; int n_imu_sensors_{0}; int n_gps_sensors_{0}; 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 d9dbde9c..bdfecf3d 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 @@ -108,7 +108,8 @@ class UkfWrapper explicit UkfWrapper( std::shared_ptr parent_node, const std::string & tf_prefix, - const std::string & plugin_name); + const std::string & plugin_name, + bool local_filter); //! @brief Destructor //! @@ -870,6 +871,7 @@ class UkfWrapper std::string plugin_name_; std::string tf_prefix_; std::vector gps_callbackData_arr_; + bool local_filter_; }; 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 b212b73f..5d00624a 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -32,10 +32,14 @@ void FusionLocalizer::on_initialize() RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'", plugin_name.c_str()); - ukf_wrapper_ = std::make_unique( - localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter" + ukf_global_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false ); - ukf_wrapper_->initialize(); + ukf_local_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true + ); + ukf_global_->initialize(); + ukf_local_->initialize(); localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_); @@ -61,7 +65,7 @@ void FusionLocalizer::on_initialize() UTM_zone_ = std::to_string(zone) + (northp ? "N" : "S"); UTM_origin_z_ = altitude_origin_; - n_gps_sensors_ = static_cast(ukf_wrapper_->getGpsCallbackDataArr().size()); + n_gps_sensors_ = static_cast(ukf_global_->getGpsCallbackDataArr().size()); RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); } @@ -81,9 +85,9 @@ void FusionLocalizer::update_rt(NavState & nav_state) // nav_state.set("UTM_gnss_pose", pose); // Call the wrapper callback const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - ukf_wrapper_->poseCallback( + ukf_global_->poseCallback( std::make_shared(pose), - ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data + ukf_global_->getGpsCallbackDataArr()[i], // callback_data tf_info.map_frame, // target_frame tf_info.odom_frame, // pose_source_frame false // imu_data @@ -92,11 +96,15 @@ void FusionLocalizer::update_rt(NavState & nav_state) } } - ukf_wrapper_->periodicUpdate(); + ukf_global_->periodicUpdate(); + ukf_local_->periodicUpdate(); - nav_msgs::msg::Odometry current_odom; - if (ukf_wrapper_->getFilteredOdometryMessage(¤t_odom)) { - nav_state.set("robot_pose", current_odom); + nav_msgs::msg::Odometry global_odom, local_odom; + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + nav_state.set("robot_pose", global_odom); + } + if (ukf_local_->getFilteredOdometryMessage(&local_odom)) { + nav_state.set("robot_pose_local", local_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 8bf14a01..71a065f0 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 @@ -86,7 +86,8 @@ using namespace std::chrono_literals; UkfWrapper::UkfWrapper( std::shared_ptr parent_node, const std::string & tf_prefix, - const std::string & plugin_name) + const std::string & plugin_name, + bool local_filter) : print_diagnostics_(true), publish_acceleration_(false), publish_transform_(true), @@ -114,7 +115,8 @@ UkfWrapper::UkfWrapper( last_set_pose_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0, 0, RCL_ROS_TIME), tf_timeout_(0ns), - tf_time_offset_(0ns) + tf_time_offset_(0ns), + local_filter_(local_filter) { parent_node_ = parent_node; tf_prefix_ = tf_prefix; @@ -895,7 +897,11 @@ void UkfWrapper::loadParams() odom_frame_id_ = tf_info.odom_frame; base_link_frame_id_ = tf_info.robot_footprint_frame; // World frame comes from Easynav TFInfo configuration - world_frame_id_ = tf_info.map_frame; + if(local_filter_) { + world_frame_id_ = tf_info.odom_frame; + } else { + world_frame_id_ = tf_info.map_frame; + } base_link_output_frame_id_ = base_link_frame_id_; @@ -2262,15 +2268,21 @@ void UkfWrapper::initialize() // Position publisher rclcpp::PublisherOptions publisher_options; publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + std::string mode_prefix; + if (local_filter_) { + mode_prefix = "local_"; + } else { + mode_prefix = "global_"; + } position_pub_ = parent_node_->create_publisher( - "odometry/filtered", rclcpp::QoS(10), publisher_options); + mode_prefix + "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); + mode_prefix + "accel/filtered", rclcpp::QoS(10), publisher_options); } // Block commented as it is handled in the update_rt From 4ee57adee0623d8d80217f0cc2de27f8f494844c Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Thu, 5 Feb 2026 13:08:40 +0100 Subject: [PATCH 138/230] 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 1f5ca1bfcfbc95115833591cb9bbc88e1ae5d365 Mon Sep 17 00:00:00 2001 From: migueldm Date: Tue, 10 Feb 2026 14:56:46 +0100 Subject: [PATCH 139/230] Several bugs fixed. Now it correctly integrates GPS measurements to the dual filter. --- .../config/example_params_summit_dual.yaml | 37 ++--- .../FusionLocalizer.hpp | 18 ++- .../easynav_fusion_localizer/ukf_wrapper.hpp | 11 +- .../FusionLocalizer.cpp | 146 +++++++++++++----- .../easynav_fusion_localizer/ukf_wrapper.cpp | 41 +++-- 5 files changed, 164 insertions(+), 89 deletions(-) diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml index 3b0e1e43..fef82461 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -35,22 +35,12 @@ localizer_node: base_link_frame: base_link world_frame: odom - # # --- 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: /robotnik_base_control/odom_fix odom0_config: - [true, true, true, + [true, true, false, false, false, true, - true, true, true, + true, true, false, false, false, true, false, false, false] odom0_differential: false @@ -59,11 +49,11 @@ localizer_node: imu0: /imu/data imu0_config: [false, false, false, - true, true, true, false, false, false, true, true, true, + true, true, true, false, false, false] - imu0_differential: true + imu0_differential: false imu0_remove_gravitational_acceleration: true # twist0: /gps/fix_velocity @@ -105,18 +95,18 @@ localizer_node: gps0: /gps/fix gps0_config: [true, true, false, - false, false, false, + false, false, true, false, false, false, false, false, false, false, false, false] gps0_differential: false - #--- INPUT 2: Wheel Odometry --- - odom0: /robotnik_base_control/odom + # --- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom_fix odom0_config: [false, false, false, false, false, false, - true, true, false, + true, true, false, false, false, true, false, false, false] odom0_differential: false @@ -125,11 +115,11 @@ localizer_node: imu0: /imu/data imu0_config: [false, false, false, - true, true, true, false, false, false, true, true, true, + true, true, true, false, false, false] - imu0_differential: true + imu0_differential: false imu0_remove_gravitational_acceleration: true # twist0: /gps/fix_velocity @@ -158,6 +148,11 @@ localizer_node: 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] + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] maps_manager_node: 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 fcafdbb7..330869b2 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -1,17 +1,21 @@ #pragma once #include +#include #include "easynav_core/LocalizerMethodBase.hpp" -#include "easynav_fusion_localizer/ukf_wrapper.hpp" // Tu wrapper refactorizado +#include "easynav_fusion_localizer/ukf_wrapper.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" namespace easynav { +constexpr double kNoOrientationCovariance = 1e5; + /** * @class FusionLocalizer * @brief Plugin de localización para EasyNav que integra el UKF de @@ -54,12 +58,14 @@ class FusionLocalizer : public easynav::LocalizerMethodBase std::unique_ptr ukf_local_{nullptr}; std::unique_ptr ukf_global_{nullptr}; - int n_imu_sensors_{0}; int n_gps_sensors_{0}; geometry_msgs::msg::PoseWithCovarianceStamped navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg); + sensor_msgs::msg::NavSatFix + odom_to_navsatfix(const nav_msgs::msg::Odometry & odom_msg); + double latitude_origin_{0.0}; double longitude_origin_{0.0}; double altitude_origin_{0.0}; @@ -67,8 +73,14 @@ class FusionLocalizer : public easynav::LocalizerMethodBase double UTM_origin_y_{0.0}; double UTM_origin_z_{0.0}; std::string UTM_zone_; + int UTM_zone_number_{0}; + bool UTM_zone_northp_{true}; + + std::vector last_gps_stamp_; - std::vector last_gps_stamp_; + rclcpp::Publisher::SharedPtr navsat_pub_{nullptr}; + rclcpp::Publisher::SharedPtr gps_debug_pub_{nullptr}; + std::string navsatfix_topic_; }; 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 bdfecf3d..1c7f4d35 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 @@ -64,7 +64,6 @@ #include "tf2/LinearMath/Transform.hpp" #include #include -#include namespace robot_localization { @@ -345,7 +344,7 @@ class UkfWrapper //! bool validateFilterOutput(nav_msgs::msg::Odometry * message); - std::vector getGpsCallbackDataArr() const + const std::vector & getGpsCallbackDataArr() const { return gps_callbackData_arr_; } @@ -821,13 +820,9 @@ class UkfWrapper //! rclcpp::Service::SharedPtr reset_srv_; - //! @brief Transform buffer for managing coordinate transforms + //! @brief Transform buffer for managing coordinate transforms (shared RTTFBuffer singleton) //! - std::unique_ptr tf_buffer_; - - //! @brief Transform listener for receiving transforms - //! - std::unique_ptr tf_listener_; + tf2_ros::Buffer * tf_buffer_{nullptr}; //! @brief broadcaster of worldTransform tfs //! 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 5d00624a..147ce87b 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -6,6 +6,7 @@ #include "easynav_common/types/IMUPerception.hpp" #include "easynav_common/types/GNSSPerception.hpp" +#include "sensor_msgs/msg/nav_sat_status.hpp" #include @@ -23,8 +24,6 @@ void FusionLocalizer::on_initialize() auto localizer_node = std::dynamic_pointer_cast(node); - last_gps_stamp_.resize(10, 0.0); - const std::string & plugin_name = this->get_plugin_name(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -48,6 +47,14 @@ void FusionLocalizer::on_initialize() localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0)); localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_); + + localizer_node->declare_parameter( + plugin_name + ".navsatfix_topic", std::string("global/navsatfix")); + localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); + navsat_pub_ = localizer_node->create_publisher( + navsatfix_topic_, rclcpp::QoS(10)); + gps_debug_pub_ = localizer_node->create_publisher( + "gps_pose", rclcpp::QoS(10)); } catch (const std::exception & e) { RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", @@ -57,40 +64,41 @@ void FusionLocalizer::on_initialize() } - 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"); + GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_, + UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); + UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S"); UTM_origin_z_ = altitude_origin_; n_gps_sensors_ = static_cast(ukf_global_->getGpsCallbackDataArr().size()); + last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME)); RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); } -// 2. Hook de actualización RT (Tu "Timer" de alta frecuencia) void FusionLocalizer::update_rt(NavState & nav_state) { - if(n_gps_sensors_ && nav_state.has("gnss")) { + EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_update_rt"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + if (n_gps_sensors_ && nav_state.has("gnss")) { auto gps_data = nav_state.get(std::string("gnss")); + const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); 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_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + continue; + } + rclcpp::Time gps_time(gps_data[i]->data.header.stamp); if (gps_time > last_gps_stamp_[i]) { - // if(true) { + EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); last_gps_stamp_[i] = gps_time; auto pose = navsatfix_to_pose(gps_data[i]->data); - // nav_state.set("UTM_gnss_pose", pose); - // Call the wrapper callback - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + gps_debug_pub_->publish(pose); ukf_global_->poseCallback( std::make_shared(pose), - ukf_global_->getGpsCallbackDataArr()[i], // callback_data - tf_info.map_frame, // target_frame - tf_info.odom_frame, // pose_source_frame - false // imu_data + gps_cb_arr[i], + tf_info.map_frame, + gps_data[i]->data.header.frame_id, + false ); } } @@ -102,32 +110,49 @@ void FusionLocalizer::update_rt(NavState & nav_state) nav_msgs::msg::Odometry global_odom, local_odom; if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { nav_state.set("robot_pose", global_odom); + navsat_pub_->publish(odom_to_navsatfix(global_odom)); } if (ukf_local_->getFilteredOdometryMessage(&local_odom)) { nav_state.set("robot_pose_local", local_odom); } } -// 3. Hook de actualización no-RT (baja frecuencia) void FusionLocalizer::update([[maybe_unused]] NavState & nav_state) { } +/** + * @brief Converts a NavSatFix message to a PoseWithCovarianceStamped in UTM coordinates. + * + * This function transforms the latitude, longitude, and altitude from a sensor_msgs::msg::NavSatFix + * message into UTM coordinates and populates a geometry_msgs::msg::PoseWithCovarianceStamped message. + * The output pose is expressed relative to a predefined UTM origin. The orientation is set to identity + * (no rotation), as orientation cannot be inferred from GPS data alone. + * + * Covariance handling: + * - If the NavSatFix message provides a diagonal covariance (COVARIANCE_TYPE_DIAGONAL_KNOWN), + * the corresponding variances are copied into the pose covariance matrix. + * - If not, a default variance of 1.0 is used for the position, and a warning is issued. + * + * @param navsat_msg The input NavSatFix message containing latitude, longitude, altitude, and covariance. + * @return geometry_msgs::msg::PoseWithCovarianceStamped The resulting pose in UTM coordinates with covariance. + * + * @note + * The function uses RCLCPP_WARN_THROTTLE to log a warning if the covariance type is unknown or invalid. + * This throttled warning will only be emitted at most once every 5 seconds, preventing log flooding, + * unlike RCLCPP_WARN which would log a warning every time the condition is met. + */ 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; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); pose_msg.header.frame_id = tf_info.map_frame; - // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y) double utm_x, utm_y; int zone; bool northp; @@ -138,7 +163,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose zone, northp, utm_x, - utm_y); + utm_y, + UTM_zone_number_); pose_msg.pose.pose.position.x = utm_x - UTM_origin_x_; pose_msg.pose.pose.position.y = utm_y - UTM_origin_y_; @@ -151,23 +177,63 @@ 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 + if (navsat_msg.position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) + { + pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0]; + pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4]; + pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; + + pose_msg.pose.covariance[21] = kNoOrientationCovariance; + pose_msg.pose.covariance[28] = kNoOrientationCovariance; + pose_msg.pose.covariance[35] = kNoOrientationCovariance; + } else { + // Fallback variances if GPS doesn't provide them + double default_var = 1.0; // 1 meter variance standard + pose_msg.pose.covariance[0] = default_var; + pose_msg.pose.covariance[7] = default_var; + pose_msg.pose.covariance[14] = default_var; + + pose_msg.pose.covariance[21] = kNoOrientationCovariance; + pose_msg.pose.covariance[28] = kNoOrientationCovariance; + pose_msg.pose.covariance[35] = kNoOrientationCovariance; + + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, + "NavSatFix covariance type unknown or invalid. Using default covariance."); + } - 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 + return pose_msg; +} - 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 +sensor_msgs::msg::NavSatFix FusionLocalizer::odom_to_navsatfix( + const nav_msgs::msg::Odometry & odom_msg) +{ + sensor_msgs::msg::NavSatFix navsat_msg; - pose_msg.pose.covariance[21] = 99999.0; - pose_msg.pose.covariance[28] = 99999.0; - pose_msg.pose.covariance[35] = 99999.0; + navsat_msg.header = odom_msg.header; + navsat_msg.header.frame_id = "gps_link"; + navsat_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + navsat_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - return pose_msg; + const double utm_x = odom_msg.pose.pose.position.x + UTM_origin_x_; + const double utm_y = odom_msg.pose.pose.position.y + UTM_origin_y_; + + double latitude = 0.0; + double longitude = 0.0; + GeographicLib::UTMUPS::Reverse( + UTM_zone_number_, UTM_zone_northp_, utm_x, utm_y, latitude, longitude); + + navsat_msg.latitude = latitude; + navsat_msg.longitude = longitude; + navsat_msg.altitude = odom_msg.pose.pose.position.z + UTM_origin_z_; + + navsat_msg.position_covariance[0] = odom_msg.pose.covariance[0]; + navsat_msg.position_covariance[4] = odom_msg.pose.covariance[7]; + navsat_msg.position_covariance[8] = odom_msg.pose.covariance[14]; + + navsat_msg.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + return navsat_msg; } } // namespace easynav 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 71a065f0..20e6ef05 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 @@ -75,7 +75,6 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include -#include #include "easynav_common/RTTFBuffer.hpp" @@ -122,8 +121,8 @@ UkfWrapper::UkfWrapper( tf_prefix_ = tf_prefix; plugin_name_ = plugin_name; - tf_buffer_ = std::make_unique(parent_node_->get_clock()); - tf_listener_ = std::make_unique(*tf_buffer_); + // Use the shared RTTFBuffer singleton instead of creating a private tf2_ros::Buffer + tf_buffer_ = easynav::RTTFBuffer::getInstance(); state_variable_names_.push_back("X"); state_variable_names_.push_back("Y"); @@ -149,8 +148,6 @@ UkfWrapper::~UkfWrapper() 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(); @@ -186,8 +183,7 @@ void UkfWrapper::reset() last_diff_time_ = parent_node_->now().seconds(); - // clear tf buffer to avoid TF_OLD_DATA errors - tf_buffer_->clear(); + // Note: tf_buffer_ is the shared RTTFBuffer singleton, do not clear it here // clear last message timestamp, so older messages will be accepted last_message_times_.clear(); @@ -372,7 +368,7 @@ void UkfWrapper::enqueueMeasurement( const std::vector & update_vector, const double mahalanobis_thresh, const rclcpp::Time & time) { - MeasurementPtr meas = MeasurementPtr(new Measurement()); + MeasurementPtr meas = std::make_shared(); meas->topic_name_ = topic_name; meas->measurement_ = measurement; @@ -1178,30 +1174,39 @@ void UkfWrapper::loadParams() "\npermit_corrected_publication is " << permit_corrected_publication_ << "\nprint_diagnostics is " << print_diagnostics_ << "\n"); + // Determine mode prefix for service/topic names to avoid collisions + // between local and global filter instances + std::string mode_prefix; + if (local_filter_) { + mode_prefix = "local_"; + } else { + mode_prefix = "global_"; + } + // Create a subscriber for manually setting/resetting pose set_pose_sub_ = parent_node_->create_subscription( - "set_pose", rclcpp::QoS(1), + mode_prefix + "set_pose", rclcpp::QoS(1), std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1), options); // Create a service for manually setting/resetting pose set_pose_service_ = parent_node_->create_service( - "set_pose", std::bind( + mode_prefix + "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( + "~/" + mode_prefix + "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( + mode_prefix + "reset", std::bind( &UkfWrapper::resetSrvCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -1209,7 +1214,7 @@ void UkfWrapper::loadParams() // publishing toggle_filter_processing_srv_ = parent_node_->create_service( - "~/toggle", std::bind( + "~/" + mode_prefix + "toggle", std::bind( &UkfWrapper::toggleFilterProcessingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -2096,6 +2101,8 @@ void UkfWrapper::odometryCallback( 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 { @@ -2876,7 +2883,7 @@ bool UkfWrapper::prepareAcceleration( // 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_, + tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_, target_frame_trans); if (can_transform) { @@ -3105,7 +3112,7 @@ bool UkfWrapper::preparePose( // 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_, + tf_buffer_, final_target_frame, pose_tmp.frame_id_, rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, target_frame_trans); @@ -3115,7 +3122,7 @@ bool UkfWrapper::preparePose( 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_, + tf_buffer_, source_frame, base_link_frame_id_, rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, source_frame_trans); } @@ -3537,7 +3544,7 @@ bool UkfWrapper::prepareTwist( // 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_, + tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_, target_frame_trans); if (can_transform) { From 767a07b4f81a177266c574b8fc6f061cae89c859 Mon Sep 17 00:00:00 2001 From: migueldm Date: Mon, 16 Feb 2026 11:36:31 +0100 Subject: [PATCH 140/230] fixed odom topic --- .../config/example_params_summit_dual.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml index fef82461..52e7b06e 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -36,7 +36,7 @@ localizer_node: world_frame: odom #--- INPUT 2: Wheel Odometry --- - odom0: /robotnik_base_control/odom_fix + odom0: /robotnik_base_control/odom odom0_config: [true, true, false, false, false, true, @@ -102,7 +102,7 @@ localizer_node: gps0_differential: false # --- INPUT 2: Wheel Odometry --- - odom0: /robotnik_base_control/odom_fix + odom0: /robotnik_base_control/odom odom0_config: [false, false, false, false, false, false, @@ -126,7 +126,7 @@ localizer_node: # twist0_config: # [false, false, false, # false, false, false, - # true, false, false, + # true, true, false, # false, false, false, # false, false, false] # twist0_differential: false 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 141/230] 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 142/230] 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 143/230] 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 8be80f56bef07edd0c42ab6151bf81ce8bb58d68 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 16:51:35 +0100 Subject: [PATCH 144/230] Dependencies were corrected --- maps_managers/easynav_octomap_maps_manager/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 002b28db..33e837e0 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -18,7 +18,6 @@ nav_msgs sensor_msgs ament_index_cpp - octomap octomap_ros octomap_msgs pcl_conversions From 297d3a4d81ecf5d8c559884da77b92630857bbc7 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 17:07:45 +0100 Subject: [PATCH 145/230] Dependencies were corrected --- .../src/easynav_mpc_controller/MPCController.cpp | 3 ++- maps_managers/easynav_octomap_maps_manager/package.xml | 1 - 2 files changed, 2 insertions(+), 2 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/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 002b28db..33e837e0 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -18,7 +18,6 @@ nav_msgs sensor_msgs ament_index_cpp - octomap octomap_ros octomap_msgs pcl_conversions From 35af9de705188641f7e687e659ae248d63b2b7f7 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 17:41:57 +0100 Subject: [PATCH 146/230] Fixed CI --- .github/workflows/kilted.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index b16834b7..6d68281e 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -22,7 +22,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_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 skip-tests: false colcon-defaults: | From 7a58a2535f5562275f2452d9decfd169eb48eea3 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 17:54:15 +0100 Subject: [PATCH 147/230] Fixed CI --- .github/workflows/kilted_cron.yaml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml index dc93850f..39a45694 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_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: false colcon-defaults: | { 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 148/230] 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 866b8190fb609245904e1d06f1e97d4f1fd1e818 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 18 Feb 2026 09:49:44 +0100 Subject: [PATCH 149/230] GPLv3 -> Apache 2.0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- LICENSE | 1076 ++++++----------- README.md | 2 +- common/easynav_costmap_common/package.xml | 2 +- .../tests/costmap_2d_tests.cpp | 22 +- .../easynav_simple_common/SimpleMap.hpp | 22 +- common/easynav_simple_common/package.xml | 2 +- .../src/easynav_simple_common/SimpleMap.cpp | 22 +- .../tests/simple_maps_tests.cpp | 22 +- controllers/easynav_mpc_controller/README.md | 2 +- .../easynav_mpc_controller/MPCController.hpp | 13 +- .../easynav_mpc_controller/MPCOptimizer.hpp | 13 +- .../easynav_mpc_controller/package.xml | 2 +- .../easynav_mpc_controller/MPCController.cpp | 22 +- .../easynav_mpc_controller/MPCOptimizer.cpp | 22 +- controllers/easynav_mppi_controller/README.md | 2 +- .../MPPIController.hpp | 13 +- .../easynav_mppi_controller/MPPIOptimizer.hpp | 13 +- .../easynav_mppi_controller/package.xml | 2 +- .../MPPIController.cpp | 22 +- .../easynav_serest_controller/README.md | 2 +- .../SerestController.hpp | 22 +- .../easynav_serest_controller/package.xml | 2 +- .../SerestController.cpp | 22 +- .../easynav_simple_controller/README.md | 2 +- .../SimpleController.hpp | 13 +- .../easynav_simple_controller/package.xml | 2 +- .../SimpleController.cpp | 34 +- .../tests/simple_controller_tests.cpp | 22 +- controllers/easynav_vff_controller/README.md | 2 +- .../easynav_vff_controller/VffController.hpp | 22 +- .../easynav_vff_controller/package.xml | 2 +- .../easynav_vff_controller/VffController.cpp | 22 +- .../easynav_costmap_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../easynav_costmap_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 26 +- .../tests/costmap_localizer_tests.cpp | 22 +- localizers/easynav_fusion_localizer/README.md | 2 +- .../easynav_fusion_localizer/package.xml | 2 +- localizers/easynav_gps_localizer/README.md | 2 +- .../easynav_gps_localizer/GpsLocalizer.hpp | 22 +- localizers/easynav_gps_localizer/package.xml | 2 +- .../easynav_gps_localizer/GpsLocalizer.cpp | 22 +- localizers/easynav_navmap_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../PerceptionModel.hpp | 22 +- .../easynav_navmap_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 22 +- .../tests/costmap_localizer_tests.cpp | 22 +- localizers/easynav_simple_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../easynav_simple_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 25 +- .../tests/simple_localizer_tests.cpp | 22 +- .../easynav_bonxai_maps_manager/README.md | 2 +- .../BonxaiMapsManager.hpp | 22 +- .../easynav_bonxai_maps_manager/package.xml | 2 +- .../BonxaiMapsManager.cpp | 22 +- .../easynav_costmap_maps_manager/README.md | 2 +- .../CostmapMapsManager.hpp | 22 +- .../filters/CostmapFilter.hpp | 22 +- .../filters/ObstacleFilter.hpp | 22 +- .../easynav_costmap_maps_manager/package.xml | 2 +- .../CostmapMapsManager.cpp | 22 +- .../filters/CostmapFilter.cpp | 22 +- .../filters/ObstacleFilter.cpp | 22 +- .../tests/costmap_mapsmanager_tests.cpp | 13 +- .../easynav_navmap_maps_manager/README.md | 2 +- .../NavMapMapsManager.hpp | 22 +- .../filters/InflationFilter.hpp | 22 +- .../filters/NavMapFilter.hpp | 22 +- .../filters/ObstacleFilter.hpp | 22 +- .../easynav_navmap_maps_manager/package.xml | 2 +- .../NavMapMapsManager.cpp | 22 +- .../filters/NavMapFilter.cpp | 22 +- .../filters/ObstacleFilter.cpp | 22 +- .../tests/navmap_mapsmanager_tests.cpp | 13 +- .../easynav_octomap_maps_manager/README.md | 2 +- .../OctomapMapsManager.hpp | 22 +- .../filters/InflationFilter.hpp | 22 +- .../filters/ObstacleFilter.hpp | 22 +- .../filters/OctomapFilter.hpp | 22 +- .../easynav_octomap_maps_manager/package.xml | 2 +- .../OctomapMapsManager.cpp | 22 +- .../filters/ObstacleFilter.cpp | 22 +- .../filters/OctomapFilter.cpp | 22 +- .../easynav_routes_maps_manager/README.md | 2 +- .../RoutesFilter.hpp | 22 +- .../RoutesMapsManager.hpp | 22 +- .../filters/RoutesCostmapFilter.hpp | 22 +- .../easynav_routes_maps_manager/package.xml | 2 +- .../RoutesMapsManager.cpp | 22 +- .../filters/RoutesCostmapFilter.cpp | 22 +- .../tests/routes_costmap_filter_tests.cpp | 22 +- .../tests/routes_mapsmanager_tests.cpp | 22 +- .../easynav_simple_maps_manager/README.md | 2 +- .../SimpleMapsManager.hpp | 22 +- .../easynav_simple_maps_manager/package.xml | 2 +- .../SimpleMapsManager.cpp | 36 +- .../tests/simple_mapsmanager_tests.cpp | 22 +- planners/easynav_costmap_planner/README.md | 2 +- .../CostmapPlanner.hpp | 22 +- planners/easynav_costmap_planner/package.xml | 2 +- .../CostmapPlanner.cpp | 22 +- planners/easynav_navmap_planner/README.md | 2 +- .../easynav_navmap_planner/AStarPlanner.hpp | 22 +- planners/easynav_navmap_planner/package.xml | 2 +- .../easynav_navmap_planner/AStarPlanner.cpp | 22 +- planners/easynav_simple_planner/README.md | 2 +- .../easynav_simple_planner/SimplePlanner.hpp | 22 +- planners/easynav_simple_planner/package.xml | 2 +- .../easynav_simple_planner/SimplePlanner.cpp | 33 +- 112 files changed, 1117 insertions(+), 1562 deletions(-) diff --git a/LICENSE b/LICENSE index f288702d..c2334c6c 100644 --- a/LICENSE +++ b/LICENSE @@ -1,674 +1,402 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This 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 . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md index 583ff9a6..8d9c02fb 100644 --- a/README.md +++ b/README.md @@ -75,4 +75,4 @@ 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 **Apache-2.0** unless stated otherwise in the individual package. diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 55db3489..7f3c271f 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -11,7 +11,7 @@ Juan Sebastián Cely Gutiérrez José Miguel Guerrero Hernández - GPL-3.0-only + Apache-2.0 Francisco Martín Rico Francisco Miguel Moreno Olivo diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index 955f9514..a9ff17b8 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include "easynav_costmap_common/costmap_2d.hpp" 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 c34ad138..4039c221 100644 --- a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp +++ b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimpleMap type. diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index 63b5bae1..38ac4fb7 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Common package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 e00da478..a525d868 100644 --- a/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp +++ b/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/common/easynav_simple_common/tests/simple_maps_tests.cpp b/common/easynav_simple_common/tests/simple_maps_tests.cpp index 7ad1b184..09cd11bb 100644 --- a/common/easynav_simple_common/tests/simple_maps_tests.cpp +++ b/common/easynav_simple_common/tests/simple_maps_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/controllers/easynav_mpc_controller/README.md b/controllers/easynav_mpc_controller/README.md index a707baee..9d6fbbb3 100644 --- a/controllers/easynav_mpc_controller/README.md +++ b/controllers/easynav_mpc_controller/README.md @@ -61,4 +61,4 @@ This package does not create service servers or clients. This controller does not explicitly publish or require TF frames in code. ## License -GPL-3.0-only +Apache-2.0 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 c8c31066..515ac3fb 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef 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 index 230ed320..716225be 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index c1ec0cb3..6658226d 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -5,7 +5,7 @@ 0.0.1 Easy Navigation: MPC Controller package. Juan S. Cely G. - GPL-3.0-only + Apache-2.0 ament_cmake libnlopt-cxx-dev 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 939839c5..b65911a2 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the MPCController class. 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 123f1b3f..c9391bbb 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of MPCParameters and MPCOptimizer classes. diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index cf0f9e9f..4c5cdab0 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -74,4 +74,4 @@ This controller does not explicitly publish or require TF frames in code. ## License -GPL-3.0-only +Apache-2.0 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 4e34017e..0f3f03c9 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp @@ -1,8 +1,17 @@ // 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. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ #define EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ diff --git a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp index ba9ba858..d900e166 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 672069e4..c3382a3c 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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 b471b039..c0fc3471 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the MPPIController class. diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index b4b64b68..28ebaf30 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -113,4 +113,4 @@ This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). ## License -GPL-3.0-only +Apache-2.0 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 0b908341..5e0d7dee 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ #define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 76b4e613..a57476ed 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: SeReST Controller package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 2cf55119..cfed5193 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleController class. diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index 3f8f03ee..ea0d048b 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -72,4 +72,4 @@ This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). ## License -GPL-3.0-only +Apache-2.0 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 8820859d..0444c69a 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp @@ -1,8 +1,17 @@ // 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. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ #define EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 8eadc469..99181b8b 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Controller package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 9438c50f..aed490e7 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleController class. @@ -114,6 +110,14 @@ 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 & goal_pose = path.poses.back().pose; + + rclcpp::Time latest_stamp = nav_state.get("robot_pose").header.stamp; + if (rclcpp::Time(path.poses.back().header.stamp, + latest_stamp.get_clock_type()) > latest_stamp) + { + latest_stamp = rclcpp::Time(path.poses.back().header.stamp, latest_stamp.get_clock_type()); + } + double dist_to_goal = get_distance(pose, goal_pose); double angle_to_goal = get_diff_angle(pose.orientation, goal_pose.orientation); @@ -122,7 +126,7 @@ SimpleController::update_rt(NavState & nav_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_.header.stamp = latest_stamp; twist_stamped_.twist.linear.x = 0.0; twist_stamped_.twist.angular.z = 0.0; // reset PID internal state to avoid windup / residual derivative @@ -168,7 +172,7 @@ SimpleController::update_rt(NavState & nav_state) last_vrot_ = vrot; twist_stamped_.header.frame_id = path.header.frame_id; - twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.header.stamp = latest_stamp; twist_stamped_.twist.linear.x = vlin; twist_stamped_.twist.angular.z = vrot; diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index eff6b1ea..48d2a2f4 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index 299a1958..4ff60f52 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -59,4 +59,4 @@ Relies on frames stamped in `robot_pose`; does not query TF directly. ## License -GPL-3.0-only +Apache-2.0 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 f9048de6..66dd24e4 100644 --- a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp +++ b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the VffController method. diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index bf826f5e..07b23911 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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 1030276c..65d5e626 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the VffController class. diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 8e5f5e7b..c1590195 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -77,4 +77,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 44b8352b..4576dc13 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index fbd2abd3..a8d85e0b 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Costmap Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 75f51b21..30c3bbbb 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AMCLLocalizer class using Costmap2D. @@ -588,7 +584,7 @@ AMCLLocalizer::correct(NavState & nav_state) auto view = PointPerceptionsOpsView(perceptions); view.downsample(map_static.getResolution()) - .fuse(tf_info.robot_footprint_frame) + .fuse(tf_info.robot_footprint_frame, last_input_time_) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.getResolution()); @@ -599,8 +595,6 @@ AMCLLocalizer::correct(NavState & nav_state) return; } - last_input_time_ = view.get_latest_stamp(); - for (auto & particle : particles_) { int hits = 0; int possible_hits = 0; diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index 9b2fe4fc..bc03cbe6 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index 16ef81a8..e48542e7 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -51,4 +51,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 5f08b86a..6d2175c6 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -6,7 +6,7 @@ Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel - GPL-3.0-only + Apache-2.0 ament_cmake diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index 4c45be17..5306d1fb 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -59,4 +59,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 cf0192d6..03d1fcdc 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the GpsLocalizer class, a default plugin implementation for localization. diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index 6180c2e0..13d3d8d7 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -6,7 +6,7 @@ Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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..c4eec332 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the GpsLocalizer class. diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index e1451c27..b984b01b 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -87,4 +87,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 23d2c45f..c166f876 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp index 4089e0c5..b3cd099d 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_LOCALIZER__PERCEPTIONMODEL_HPP_ diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 34c0d79e..96837eff 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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..79117a91 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief AMCLLocalizer implementation (Bonxai + probabilistic inflation + ray casting). diff --git a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp index a40c68e9..7e9166a0 100644 --- a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 481d6830..bee15a05 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -75,4 +75,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 7bff4232..9ba5ebab 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index 93492165..16d61844 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 30a62e70..6fc0b92a 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AMCLLocalizer class. @@ -261,6 +257,7 @@ AMCLLocalizer::on_initialize() node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); last_reseed_ = get_node()->now(); + last_input_time_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); } @@ -388,7 +385,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_footprint_frame) + .fuse(tf_info.robot_footprint_frame, last_input_time_) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.resolution()) diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index fbb1ca18..d353bbea 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_bonxai_maps_manager/README.md b/maps_managers/easynav_bonxai_maps_manager/README.md index 6f08a413..a01fb940 100644 --- a/maps_managers/easynav_bonxai_maps_manager/README.md +++ b/maps_managers/easynav_bonxai_maps_manager/README.md @@ -68,4 +68,4 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) ## License -GPL-3.0-only +Apache-2.0 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 8a81e0f1..4130fccb 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the BonxaiMapsManager method. diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 46c75db4..1ac676b4 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 bc8c3e4f..b5222471 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index d2df322a..ebdf66c9 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -162,4 +162,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 f4d0fb58..b36ee8ff 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the CostmapMapsManager method. 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 0ede2b6a..85089a44 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_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 940a2e18..39880dd8 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 186af843..6d0cd57a 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 229846b0..ea4e4f14 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include 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 b172a165..465b2637 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 1dd019e9..b354f31e 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 0d05f16d..f14df242 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 @@ -1,8 +1,17 @@ // 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. +// 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 diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 2f223f28..9f925d2f 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -132,4 +132,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 6f7c2c1b..ed3b393c 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the NavMapMapsManager method. 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 74f1725a..7fb10eab 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 a561ac73..7c158a58 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ 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 d4c87015..3e25b011 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index 6951d89d..6b29d112 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 c230cb4b..aa0d04e4 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include 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 139278ba..df66c38c 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 cddda7ba..aeeed492 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 9de01f7e..d06b8904 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 @@ -1,8 +1,17 @@ // 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. +// 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 diff --git a/maps_managers/easynav_octomap_maps_manager/README.md b/maps_managers/easynav_octomap_maps_manager/README.md index 5d60468d..119b30f2 100644 --- a/maps_managers/easynav_octomap_maps_manager/README.md +++ b/maps_managers/easynav_octomap_maps_manager/README.md @@ -105,4 +105,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 8b4f79ee..8d84e1b1 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the OctomapMapsManager method. 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 c0524721..fae70efd 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 571433c3..d103321e 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ 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 efa8a984..087c61d4 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 002b28db..e90d59d4 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 a3fd6aad..e8e1a6c6 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 ff276efb..33a1abb9 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 b25012c6..36ad1e94 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md index 30bb391e..1bf0af58 100644 --- a/maps_managers/easynav_routes_maps_manager/README.md +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -152,4 +152,4 @@ necessary TF tree. ## License -GPL-3.0-only +Apache-2.0 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 192af482..d194e338 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ #define 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 37fb17f3..e7d56c86 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the RoutesMapsManager class and related types. 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 c910a916..7ea65635 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef 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/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index 2bcb255a..f824ed9b 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Routes MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 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 index 2601842e..4af7b510 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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_routes_maps_manager/RoutesMapsManager.hpp" 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 83908e19..10a2a897 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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_routes_maps_manager/filters/RoutesCostmapFilter.hpp" 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 40d8c72b..a6f574ca 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 665a25a7..84cac5ab 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 4590daf1..10f59ab4 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -67,4 +67,4 @@ This manager does not broadcast TF; messages are stamped directly with their fra ## License -GPL-3.0-only +Apache-2.0 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 bd9a2037..b91a0186 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimpleMapsManager method. diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 15095e77..ed304e0c 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 411cdf8d..9241f3ce 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleMapsManager class. @@ -151,11 +147,13 @@ 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(tf_info.map_frame) - .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .as_points(); + rclcpp::Time stamp; + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(dynamic_map_.resolution()) + .fuse(tf_info.map_frame, stamp, false) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}); + + const auto & fused = view.as_points(); for (const auto & p : fused) { if (dynamic_map_.check_bounds_metric(p.x, p.y)) { @@ -169,7 +167,7 @@ SimpleMapsManager::update(NavState & nav_state) dynamic_map_.to_occupancy_grid(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 = stamp; 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 f46ee0fe..82c67758 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index d21e367c..6750fca3 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -110,4 +110,4 @@ This plugin does not create subscriptions or services directly; it reads inputs --- ## License -GPL-3.0-only +Apache-2.0 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 0b13806b..67de89db 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the CostmapPlanner class implementing A* path planning using Costmap2D. diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 053b985f..8d4eaa2c 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Costmap planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 2f9b356c..4062a6d8 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the CostmapPlanner class using A* on Costmap2D. diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 8e48b74e..6f5f891e 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -76,4 +76,4 @@ This plugin does not create subscriptions or services directly; it retrieves all The planner assumes frame consistency between NavMap, robot pose, and goals. No TF lookups are performed internally. ## License -GPL-3.0-only +Apache-2.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 ce01f50c..3b0cc064 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AStarPlanner class implementing A* path planning using ::navmap::NavMap. diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 3376dd9e..78db89d5 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: navmap planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 0f1e7118..3f65c574 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AStarPlanner class using A* on ::navmap::NavMap (triangle graph). diff --git a/planners/easynav_simple_planner/README.md b/planners/easynav_simple_planner/README.md index 98335ace..324dc632 100644 --- a/planners/easynav_simple_planner/README.md +++ b/planners/easynav_simple_planner/README.md @@ -56,4 +56,4 @@ This plugin does not create subscriptions or services directly; it reads inputs This plugin does not perform TF lookups directly; frame consistency is assumed between map, robot pose, and published path. ## License -GPL-3.0-only +Apache-2.0 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 48bda79e..985f782f 100644 --- a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp +++ b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimplePlanner class implementing A* path planning. diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index e092f111..f40d8b64 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 7d116d20..a619ecc8 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include @@ -134,6 +130,13 @@ SimplePlanner::update(NavState & nav_state) const auto & goal = goals.goals.front().pose; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + rclcpp::Time latest_stamp = robot_pose.header.stamp; + if (rclcpp::Time(goals.goals.front().header.stamp, + latest_stamp.get_clock_type()) > latest_stamp) + { + latest_stamp = rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()); + } + auto downsampled_map = map_typed.downsample(0.2); if (goals.header.frame_id != tf_info.map_frame) { @@ -155,13 +158,13 @@ SimplePlanner::update(NavState & nav_state) downsampled_map->resolution()); if (!poses.empty()) { - current_path_.header.stamp = get_node()->now(); + current_path_.header.stamp = latest_stamp; current_path_.header.frame_id = goals.header.frame_id; for (const auto & pose : poses) { geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.header.frame_id = goals.header.frame_id; - pose_stamped.header.stamp = get_node()->now(); + pose_stamped.header.stamp = latest_stamp; pose_stamped.pose = pose; current_path_.poses.push_back(pose_stamped); } 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 150/230] 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 From 993d98acca2e2cd9505d2721111accc092391d06 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 18 Feb 2026 10:06:19 +0100 Subject: [PATCH 151/230] CI Updated --- .github/workflows/jazzy.yaml | 3 +-- .github/workflows/jazzy_cron.yaml | 4 +--- .github/workflows/kilted.yaml | 2 +- .github/workflows/kilted_cron.yaml | 2 +- .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 11301a4d..365cdccf 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_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml index ac2a5efd..c7049a98 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_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 - ref: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 6d68281e..0a4f68ed 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -22,7 +22,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: kilted skip-tests: false colcon-defaults: | diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml index 39a45694..c4b6b82b 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.github/workflows/kilted_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: kilted 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 From f2e085c6cf146e2c5b8aaa0899208476b641cd7b Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 18 Feb 2026 10:19:49 +0100 Subject: [PATCH 152/230] CI Updated --- .github/workflows/jazzy.yaml | 3 +-- .github/workflows/jazzy_cron.yaml | 4 +--- .github/workflows/kilted.yaml | 3 +-- .github/workflows/kilted_cron.yaml | 4 +--- .github/workflows/rolling_cron.yaml | 2 +- 5 files changed, 5 insertions(+), 11 deletions(-) diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 11301a4d..365cdccf 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_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml index ac2a5efd..c7049a98 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_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 - ref: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos 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 From d90bc977fa663f8a0d671b6f9ec51ec85350f7b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 19 Feb 2026 17:24:07 +0100 Subject: [PATCH 153/230] GPLv3 -> Apache 2.0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- LICENSE | 875 ++++-------------- README.md | 2 +- common/easynav_costmap_common/package.xml | 2 +- .../tests/costmap_2d_tests.cpp | 22 +- .../easynav_simple_common/SimpleMap.hpp | 22 +- common/easynav_simple_common/package.xml | 2 +- .../src/easynav_simple_common/SimpleMap.cpp | 22 +- .../tests/simple_maps_tests.cpp | 22 +- controllers/easynav_mpc_controller/README.md | 2 +- .../easynav_mpc_controller/MPCController.hpp | 13 +- .../easynav_mpc_controller/MPCOptimizer.hpp | 13 +- .../easynav_mpc_controller/package.xml | 2 +- .../easynav_mpc_controller/MPCController.cpp | 22 +- .../easynav_mpc_controller/MPCOptimizer.cpp | 22 +- controllers/easynav_mppi_controller/README.md | 2 +- .../MPPIController.hpp | 13 +- .../easynav_mppi_controller/MPPIOptimizer.hpp | 13 +- .../easynav_mppi_controller/package.xml | 2 +- .../MPPIController.cpp | 22 +- .../easynav_serest_controller/README.md | 2 +- .../SerestController.hpp | 22 +- .../easynav_serest_controller/package.xml | 2 +- .../SerestController.cpp | 22 +- .../easynav_simple_controller/README.md | 2 +- .../SimpleController.hpp | 13 +- .../easynav_simple_controller/package.xml | 2 +- .../SimpleController.cpp | 22 +- .../tests/simple_controller_tests.cpp | 22 +- controllers/easynav_vff_controller/README.md | 2 +- .../easynav_vff_controller/VffController.hpp | 22 +- .../easynav_vff_controller/package.xml | 2 +- .../easynav_vff_controller/VffController.cpp | 22 +- .../easynav_costmap_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../easynav_costmap_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 22 +- .../tests/costmap_localizer_tests.cpp | 22 +- localizers/easynav_fusion_localizer/README.md | 2 +- .../easynav_fusion_localizer/package.xml | 2 +- localizers/easynav_gps_localizer/README.md | 2 +- .../easynav_gps_localizer/GpsLocalizer.hpp | 22 +- localizers/easynav_gps_localizer/package.xml | 2 +- .../easynav_gps_localizer/GpsLocalizer.cpp | 22 +- localizers/easynav_navmap_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../PerceptionModel.hpp | 22 +- .../easynav_navmap_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 22 +- .../tests/costmap_localizer_tests.cpp | 22 +- localizers/easynav_simple_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../easynav_simple_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 22 +- .../tests/simple_localizer_tests.cpp | 22 +- .../easynav_bonxai_maps_manager/README.md | 2 +- .../BonxaiMapsManager.hpp | 22 +- .../easynav_bonxai_maps_manager/package.xml | 2 +- .../BonxaiMapsManager.cpp | 22 +- .../easynav_costmap_maps_manager/README.md | 2 +- .../CostmapMapsManager.hpp | 22 +- .../filters/CostmapFilter.hpp | 22 +- .../filters/ObstacleFilter.hpp | 22 +- .../easynav_costmap_maps_manager/package.xml | 2 +- .../CostmapMapsManager.cpp | 22 +- .../filters/CostmapFilter.cpp | 22 +- .../filters/ObstacleFilter.cpp | 22 +- .../tests/costmap_mapsmanager_tests.cpp | 13 +- .../easynav_navmap_maps_manager/README.md | 2 +- .../NavMapMapsManager.hpp | 22 +- .../filters/InflationFilter.hpp | 22 +- .../filters/NavMapFilter.hpp | 22 +- .../filters/ObstacleFilter.hpp | 22 +- .../easynav_navmap_maps_manager/package.xml | 2 +- .../NavMapMapsManager.cpp | 22 +- .../filters/NavMapFilter.cpp | 22 +- .../filters/ObstacleFilter.cpp | 22 +- .../tests/navmap_mapsmanager_tests.cpp | 13 +- .../easynav_octomap_maps_manager/README.md | 2 +- .../OctomapMapsManager.hpp | 22 +- .../filters/InflationFilter.hpp | 22 +- .../filters/ObstacleFilter.hpp | 22 +- .../filters/OctomapFilter.hpp | 22 +- .../easynav_octomap_maps_manager/package.xml | 2 +- .../OctomapMapsManager.cpp | 22 +- .../filters/ObstacleFilter.cpp | 22 +- .../filters/OctomapFilter.cpp | 22 +- .../easynav_routes_maps_manager/README.md | 2 +- .../RoutesFilter.hpp | 22 +- .../RoutesMapsManager.hpp | 22 +- .../filters/RoutesCostmapFilter.hpp | 22 +- .../easynav_routes_maps_manager/package.xml | 2 +- .../RoutesMapsManager.cpp | 22 +- .../filters/RoutesCostmapFilter.cpp | 22 +- .../tests/routes_costmap_filter_tests.cpp | 22 +- .../tests/routes_mapsmanager_tests.cpp | 22 +- .../easynav_simple_maps_manager/README.md | 2 +- .../SimpleMapsManager.hpp | 22 +- .../easynav_simple_maps_manager/package.xml | 2 +- .../SimpleMapsManager.cpp | 22 +- .../tests/simple_mapsmanager_tests.cpp | 22 +- planners/easynav_costmap_planner/README.md | 2 +- .../CostmapPlanner.hpp | 22 +- planners/easynav_costmap_planner/package.xml | 2 +- .../CostmapPlanner.cpp | 22 +- planners/easynav_navmap_planner/README.md | 2 +- .../easynav_navmap_planner/AStarPlanner.hpp | 22 +- planners/easynav_navmap_planner/package.xml | 2 +- .../easynav_navmap_planner/AStarPlanner.cpp | 22 +- planners/easynav_simple_planner/README.md | 2 +- .../easynav_simple_planner/SimplePlanner.hpp | 22 +- planners/easynav_simple_planner/package.xml | 2 +- .../easynav_simple_planner/SimplePlanner.cpp | 22 +- 112 files changed, 886 insertions(+), 1548 deletions(-) diff --git a/LICENSE b/LICENSE index f288702d..261eeb9e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,674 +1,201 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This 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 . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md index 583ff9a6..8d9c02fb 100644 --- a/README.md +++ b/README.md @@ -75,4 +75,4 @@ 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 **Apache-2.0** unless stated otherwise in the individual package. diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 55db3489..7f3c271f 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -11,7 +11,7 @@ Juan Sebastián Cely Gutiérrez José Miguel Guerrero Hernández - GPL-3.0-only + Apache-2.0 Francisco Martín Rico Francisco Miguel Moreno Olivo diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index 955f9514..a9ff17b8 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include "easynav_costmap_common/costmap_2d.hpp" 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 c34ad138..4039c221 100644 --- a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp +++ b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimpleMap type. diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index 63b5bae1..38ac4fb7 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Common package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 e00da478..a525d868 100644 --- a/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp +++ b/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/common/easynav_simple_common/tests/simple_maps_tests.cpp b/common/easynav_simple_common/tests/simple_maps_tests.cpp index 7ad1b184..09cd11bb 100644 --- a/common/easynav_simple_common/tests/simple_maps_tests.cpp +++ b/common/easynav_simple_common/tests/simple_maps_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/controllers/easynav_mpc_controller/README.md b/controllers/easynav_mpc_controller/README.md index a707baee..9d6fbbb3 100644 --- a/controllers/easynav_mpc_controller/README.md +++ b/controllers/easynav_mpc_controller/README.md @@ -61,4 +61,4 @@ This package does not create service servers or clients. This controller does not explicitly publish or require TF frames in code. ## License -GPL-3.0-only +Apache-2.0 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 c8c31066..515ac3fb 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef 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 index 230ed320..716225be 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index c1ec0cb3..6658226d 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -5,7 +5,7 @@ 0.0.1 Easy Navigation: MPC Controller package. Juan S. Cely G. - GPL-3.0-only + Apache-2.0 ament_cmake libnlopt-cxx-dev 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 939839c5..b65911a2 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the MPCController class. 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 123f1b3f..c9391bbb 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of MPCParameters and MPCOptimizer classes. diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index cf0f9e9f..4c5cdab0 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -74,4 +74,4 @@ This controller does not explicitly publish or require TF frames in code. ## License -GPL-3.0-only +Apache-2.0 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 4e34017e..0f3f03c9 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp @@ -1,8 +1,17 @@ // 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. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ #define EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ diff --git a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp index ba9ba858..d900e166 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 672069e4..c3382a3c 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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 b471b039..c0fc3471 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the MPPIController class. diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index b4b64b68..28ebaf30 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -113,4 +113,4 @@ This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). ## License -GPL-3.0-only +Apache-2.0 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 0b908341..5e0d7dee 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ #define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 76b4e613..a57476ed 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: SeReST Controller package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 b0fb4e0e..fc16c3ca 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleController class. diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index 3f8f03ee..ea0d048b 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -72,4 +72,4 @@ This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). ## License -GPL-3.0-only +Apache-2.0 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 8820859d..0444c69a 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp @@ -1,8 +1,17 @@ // 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. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ #define EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 8eadc469..99181b8b 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Controller package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 9438c50f..82679240 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleController class. diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index 4af0d5c2..1b203f2c 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index 299a1958..4ff60f52 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -59,4 +59,4 @@ Relies on frames stamped in `robot_pose`; does not query TF directly. ## License -GPL-3.0-only +Apache-2.0 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 f9048de6..66dd24e4 100644 --- a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp +++ b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the VffController method. diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index bf826f5e..07b23911 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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..37bc6d48 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the VffController class. diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 8e5f5e7b..c1590195 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -77,4 +77,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 44b8352b..4576dc13 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index fbd2abd3..a8d85e0b 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Costmap Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 48bc71ea..9779bb19 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AMCLLocalizer class using Costmap2D. diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index 9b2fe4fc..bc03cbe6 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index 16ef81a8..e48542e7 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -51,4 +51,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 5f08b86a..6d2175c6 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -6,7 +6,7 @@ Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel - GPL-3.0-only + Apache-2.0 ament_cmake diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index 4c45be17..5306d1fb 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -59,4 +59,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 cf0192d6..03d1fcdc 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the GpsLocalizer class, a default plugin implementation for localization. diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index 6180c2e0..13d3d8d7 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -6,7 +6,7 @@ Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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..cdc354f8 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the GpsLocalizer class. diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index e1451c27..b984b01b 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -87,4 +87,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 23d2c45f..c166f876 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp index 4089e0c5..b3cd099d 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_LOCALIZER__PERCEPTIONMODEL_HPP_ diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 34c0d79e..96837eff 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 c7615292..2c2a4afc 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief AMCLLocalizer implementation (Bonxai + probabilistic inflation + ray casting). diff --git a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp index a40c68e9..7e9166a0 100644 --- a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 481d6830..bee15a05 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -75,4 +75,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 7bff4232..9ba5ebab 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index 93492165..16d61844 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 693d006e..691ccb0c 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AMCLLocalizer class. diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index 731a256e..67475817 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_bonxai_maps_manager/README.md b/maps_managers/easynav_bonxai_maps_manager/README.md index 6f08a413..a01fb940 100644 --- a/maps_managers/easynav_bonxai_maps_manager/README.md +++ b/maps_managers/easynav_bonxai_maps_manager/README.md @@ -68,4 +68,4 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) ## License -GPL-3.0-only +Apache-2.0 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 8a81e0f1..4130fccb 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the BonxaiMapsManager method. diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 46c75db4..1ac676b4 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 bc8c3e4f..b5222471 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index d2df322a..ebdf66c9 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -162,4 +162,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 f4d0fb58..b36ee8ff 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the CostmapMapsManager method. 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 0ede2b6a..85089a44 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_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 940a2e18..39880dd8 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 186af843..6d0cd57a 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 229846b0..ea4e4f14 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include 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 b172a165..465b2637 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 8a9d3401..89217007 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 0d05f16d..f14df242 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 @@ -1,8 +1,17 @@ // 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. +// 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 diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 2f223f28..9f925d2f 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -132,4 +132,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 6f7c2c1b..ed3b393c 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the NavMapMapsManager method. 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 74f1725a..7fb10eab 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 a561ac73..7c158a58 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ 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 d4c87015..3e25b011 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index 6951d89d..6b29d112 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 c230cb4b..aa0d04e4 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include 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 139278ba..df66c38c 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 cddda7ba..aeeed492 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 9de01f7e..d06b8904 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 @@ -1,8 +1,17 @@ // 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. +// 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 diff --git a/maps_managers/easynav_octomap_maps_manager/README.md b/maps_managers/easynav_octomap_maps_manager/README.md index 5d60468d..119b30f2 100644 --- a/maps_managers/easynav_octomap_maps_manager/README.md +++ b/maps_managers/easynav_octomap_maps_manager/README.md @@ -105,4 +105,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 8b4f79ee..8d84e1b1 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the OctomapMapsManager method. 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 c0524721..fae70efd 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 571433c3..d103321e 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ 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 efa8a984..087c61d4 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 33e837e0..158a6971 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 a3fd6aad..e8e1a6c6 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 ff276efb..33a1abb9 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 b25012c6..36ad1e94 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md index 30bb391e..1bf0af58 100644 --- a/maps_managers/easynav_routes_maps_manager/README.md +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -152,4 +152,4 @@ necessary TF tree. ## License -GPL-3.0-only +Apache-2.0 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 192af482..d194e338 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ #define 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 37fb17f3..e7d56c86 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the RoutesMapsManager class and related types. 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 c910a916..7ea65635 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef 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/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index 2bcb255a..f824ed9b 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Routes MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 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 index 2601842e..4af7b510 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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_routes_maps_manager/RoutesMapsManager.hpp" 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 83908e19..10a2a897 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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_routes_maps_manager/filters/RoutesCostmapFilter.hpp" 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 40d8c72b..a6f574ca 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 665a25a7..84cac5ab 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 4590daf1..10f59ab4 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -67,4 +67,4 @@ This manager does not broadcast TF; messages are stamped directly with their fra ## License -GPL-3.0-only +Apache-2.0 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 bd9a2037..b91a0186 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimpleMapsManager method. diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 15095e77..ed304e0c 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 411cdf8d..efd583d1 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleMapsManager class. 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 5e5b1006..6faec033 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index d21e367c..6750fca3 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -110,4 +110,4 @@ This plugin does not create subscriptions or services directly; it reads inputs --- ## License -GPL-3.0-only +Apache-2.0 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 0b13806b..67de89db 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the CostmapPlanner class implementing A* path planning using Costmap2D. diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 053b985f..8d4eaa2c 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Costmap planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 2f9b356c..4062a6d8 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the CostmapPlanner class using A* on Costmap2D. diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 8e48b74e..6f5f891e 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -76,4 +76,4 @@ This plugin does not create subscriptions or services directly; it retrieves all The planner assumes frame consistency between NavMap, robot pose, and goals. No TF lookups are performed internally. ## License -GPL-3.0-only +Apache-2.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 ce01f50c..3b0cc064 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AStarPlanner class implementing A* path planning using ::navmap::NavMap. diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 3376dd9e..78db89d5 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: navmap planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 0f1e7118..3f65c574 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AStarPlanner class using A* on ::navmap::NavMap (triangle graph). diff --git a/planners/easynav_simple_planner/README.md b/planners/easynav_simple_planner/README.md index 98335ace..324dc632 100644 --- a/planners/easynav_simple_planner/README.md +++ b/planners/easynav_simple_planner/README.md @@ -56,4 +56,4 @@ This plugin does not create subscriptions or services directly; it reads inputs This plugin does not perform TF lookups directly; frame consistency is assumed between map, robot pose, and published path. ## License -GPL-3.0-only +Apache-2.0 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 48bda79e..985f782f 100644 --- a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp +++ b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimplePlanner class implementing A* path planning. diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index e092f111..f40d8b64 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 7d116d20..3e696190 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include From 3414cc7344f0887fa15c65e16b1d4a28f1f0a6bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 19 Feb 2026 17:39:24 +0100 Subject: [PATCH 154/230] GPLv3 -> Apache 2.0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- LICENSE | 875 ++++-------------- README.md | 2 +- common/easynav_costmap_common/package.xml | 2 +- .../tests/costmap_2d_tests.cpp | 22 +- .../easynav_simple_common/SimpleMap.hpp | 22 +- common/easynav_simple_common/package.xml | 2 +- .../src/easynav_simple_common/SimpleMap.cpp | 23 +- .../tests/simple_maps_tests.cpp | 22 +- controllers/easynav_mpc_controller/README.md | 2 +- .../easynav_mpc_controller/MPCController.hpp | 13 +- .../easynav_mpc_controller/MPCOptimizer.hpp | 13 +- .../easynav_mpc_controller/package.xml | 2 +- .../easynav_mpc_controller/MPCController.cpp | 22 +- .../easynav_mpc_controller/MPCOptimizer.cpp | 22 +- controllers/easynav_mppi_controller/README.md | 2 +- .../MPPIController.hpp | 13 +- .../easynav_mppi_controller/MPPIOptimizer.hpp | 13 +- .../easynav_mppi_controller/package.xml | 2 +- .../MPPIController.cpp | 22 +- .../easynav_serest_controller/README.md | 2 +- .../SerestController.hpp | 22 +- .../easynav_serest_controller/package.xml | 2 +- .../SerestController.cpp | 22 +- .../easynav_simple_controller/README.md | 2 +- .../SimpleController.hpp | 13 +- .../easynav_simple_controller/package.xml | 2 +- .../SimpleController.cpp | 22 +- .../tests/simple_controller_tests.cpp | 22 +- controllers/easynav_vff_controller/README.md | 2 +- .../easynav_vff_controller/VffController.hpp | 22 +- .../easynav_vff_controller/package.xml | 2 +- .../easynav_vff_controller/VffController.cpp | 22 +- .../easynav_costmap_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../easynav_costmap_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 22 +- .../tests/costmap_localizer_tests.cpp | 22 +- localizers/easynav_fusion_localizer/README.md | 2 +- .../easynav_fusion_localizer/package.xml | 2 +- localizers/easynav_gps_localizer/README.md | 2 +- .../easynav_gps_localizer/GpsLocalizer.hpp | 22 +- localizers/easynav_gps_localizer/package.xml | 2 +- .../easynav_gps_localizer/GpsLocalizer.cpp | 22 +- localizers/easynav_navmap_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../PerceptionModel.hpp | 23 +- .../easynav_navmap_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 22 +- .../tests/costmap_localizer_tests.cpp | 22 +- localizers/easynav_simple_localizer/README.md | 2 +- .../AMCLLocalizer.hpp | 22 +- .../easynav_simple_localizer/package.xml | 2 +- .../AMCLLocalizer.cpp | 22 +- .../tests/simple_localizer_tests.cpp | 22 +- .../easynav_bonxai_maps_manager/README.md | 2 +- .../BonxaiMapsManager.hpp | 22 +- .../easynav_bonxai_maps_manager/package.xml | 2 +- .../BonxaiMapsManager.cpp | 22 +- .../easynav_costmap_maps_manager/README.md | 2 +- .../CostmapMapsManager.hpp | 22 +- .../filters/CostmapFilter.hpp | 23 +- .../filters/ObstacleFilter.hpp | 23 +- .../easynav_costmap_maps_manager/package.xml | 2 +- .../CostmapMapsManager.cpp | 22 +- .../filters/CostmapFilter.cpp | 23 +- .../filters/ObstacleFilter.cpp | 23 +- .../tests/costmap_mapsmanager_tests.cpp | 13 +- .../easynav_navmap_maps_manager/README.md | 2 +- .../NavMapMapsManager.hpp | 22 +- .../filters/InflationFilter.hpp | 23 +- .../filters/NavMapFilter.hpp | 23 +- .../filters/ObstacleFilter.hpp | 23 +- .../easynav_navmap_maps_manager/package.xml | 2 +- .../NavMapMapsManager.cpp | 22 +- .../filters/NavMapFilter.cpp | 23 +- .../filters/ObstacleFilter.cpp | 23 +- .../tests/navmap_mapsmanager_tests.cpp | 13 +- .../easynav_octomap_maps_manager/README.md | 2 +- .../OctomapMapsManager.hpp | 22 +- .../filters/InflationFilter.hpp | 23 +- .../filters/ObstacleFilter.hpp | 23 +- .../filters/OctomapFilter.hpp | 23 +- .../easynav_octomap_maps_manager/package.xml | 2 +- .../OctomapMapsManager.cpp | 22 +- .../filters/ObstacleFilter.cpp | 23 +- .../filters/OctomapFilter.cpp | 23 +- .../easynav_routes_maps_manager/README.md | 2 +- .../RoutesFilter.hpp | 22 +- .../RoutesMapsManager.hpp | 22 +- .../filters/RoutesCostmapFilter.hpp | 22 +- .../easynav_routes_maps_manager/package.xml | 2 +- .../RoutesMapsManager.cpp | 23 +- .../filters/RoutesCostmapFilter.cpp | 23 +- .../tests/routes_costmap_filter_tests.cpp | 22 +- .../tests/routes_mapsmanager_tests.cpp | 23 +- .../easynav_simple_maps_manager/README.md | 2 +- .../SimpleMapsManager.hpp | 22 +- .../easynav_simple_maps_manager/package.xml | 2 +- .../SimpleMapsManager.cpp | 22 +- .../tests/simple_mapsmanager_tests.cpp | 22 +- planners/easynav_costmap_planner/README.md | 2 +- .../CostmapPlanner.hpp | 22 +- planners/easynav_costmap_planner/package.xml | 2 +- .../CostmapPlanner.cpp | 22 +- planners/easynav_navmap_planner/README.md | 2 +- .../easynav_navmap_planner/AStarPlanner.hpp | 22 +- planners/easynav_navmap_planner/package.xml | 2 +- .../easynav_navmap_planner/AStarPlanner.cpp | 22 +- planners/easynav_simple_planner/README.md | 2 +- .../easynav_simple_planner/SimplePlanner.hpp | 22 +- planners/easynav_simple_planner/package.xml | 2 +- .../easynav_simple_planner/SimplePlanner.cpp | 22 +- 112 files changed, 886 insertions(+), 1567 deletions(-) diff --git a/LICENSE b/LICENSE index f288702d..261eeb9e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,674 +1,201 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This 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 . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md index 583ff9a6..8d9c02fb 100644 --- a/README.md +++ b/README.md @@ -75,4 +75,4 @@ 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 **Apache-2.0** unless stated otherwise in the individual package. diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 55db3489..7f3c271f 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -11,7 +11,7 @@ Juan Sebastián Cely Gutiérrez José Miguel Guerrero Hernández - GPL-3.0-only + Apache-2.0 Francisco Martín Rico Francisco Miguel Moreno Olivo diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index 955f9514..a9ff17b8 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include "easynav_costmap_common/costmap_2d.hpp" 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 c34ad138..4039c221 100644 --- a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp +++ b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimpleMap type. diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index 63b5bae1..38ac4fb7 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Common package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 e00da478..e52eeaec 100644 --- a/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp +++ b/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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 #include diff --git a/common/easynav_simple_common/tests/simple_maps_tests.cpp b/common/easynav_simple_common/tests/simple_maps_tests.cpp index 7ad1b184..09cd11bb 100644 --- a/common/easynav_simple_common/tests/simple_maps_tests.cpp +++ b/common/easynav_simple_common/tests/simple_maps_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/controllers/easynav_mpc_controller/README.md b/controllers/easynav_mpc_controller/README.md index a707baee..9d6fbbb3 100644 --- a/controllers/easynav_mpc_controller/README.md +++ b/controllers/easynav_mpc_controller/README.md @@ -61,4 +61,4 @@ This package does not create service servers or clients. This controller does not explicitly publish or require TF frames in code. ## License -GPL-3.0-only +Apache-2.0 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 cee5a66d..ebb7fb6f 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef 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 index 230ed320..716225be 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index c1ec0cb3..6658226d 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -5,7 +5,7 @@ 0.0.1 Easy Navigation: MPC Controller package. Juan S. Cely G. - GPL-3.0-only + Apache-2.0 ament_cmake libnlopt-cxx-dev 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 66ec8eea..36116946 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the MPCController class. 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 123f1b3f..c9391bbb 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of MPCParameters and MPCOptimizer classes. diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index b6d7f1e9..238d8b44 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -77,4 +77,4 @@ This controller does not explicitly publish or require TF frames in code. ## License -GPL-3.0-only +Apache-2.0 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 4e34017e..0f3f03c9 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp @@ -1,8 +1,17 @@ // 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. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ #define EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ diff --git a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp index ba9ba858..d900e166 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp @@ -1,8 +1,17 @@ // 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. +// 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. // #pragma once #ifndef EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 672069e4..c3382a3c 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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 b471b039..c0fc3471 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the MPPIController class. diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index 8a36df77..9546f28e 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -117,4 +117,4 @@ This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). ## License -GPL-3.0-only +Apache-2.0 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 0b908341..5e0d7dee 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ #define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 76b4e613..a57476ed 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: SeReST Controller package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 b0fb4e0e..fc16c3ca 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleController class. diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index e9ae782d..43524dbc 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -73,4 +73,4 @@ This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). ## License -GPL-3.0-only +Apache-2.0 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 8820859d..0444c69a 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp @@ -1,8 +1,17 @@ // 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. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ #define EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 8eadc469..99181b8b 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Controller package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 9438c50f..82679240 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleController class. diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index 4af0d5c2..1b203f2c 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index 299a1958..4ff60f52 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -59,4 +59,4 @@ Relies on frames stamped in `robot_pose`; does not query TF directly. ## License -GPL-3.0-only +Apache-2.0 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 f9048de6..66dd24e4 100644 --- a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp +++ b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the VffController method. diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index bf826f5e..07b23911 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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 594ae7c4..acd0290c 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the VffController class. diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 8e5f5e7b..c1590195 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -77,4 +77,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 44b8352b..4576dc13 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index fbd2abd3..a8d85e0b 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Costmap Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 48bc71ea..9779bb19 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AMCLLocalizer class using Costmap2D. diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index 9b2fe4fc..bc03cbe6 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index 16ef81a8..e48542e7 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -51,4 +51,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 5f08b86a..6d2175c6 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -6,7 +6,7 @@ Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel - GPL-3.0-only + Apache-2.0 ament_cmake diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index 4c45be17..5306d1fb 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -59,4 +59,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 cf0192d6..03d1fcdc 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the GpsLocalizer class, a default plugin implementation for localization. diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index 6180c2e0..13d3d8d7 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -6,7 +6,7 @@ Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez - GPL-3.0-only + Apache-2.0 ament_cmake 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..cdc354f8 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the GpsLocalizer class. diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index e1451c27..b984b01b 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -87,4 +87,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 23d2c45f..c166f876 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp index 4089e0c5..ffd56be4 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_LOCALIZER__PERCEPTIONMODEL_HPP_ #define EASYNAV_NAVMAP_LOCALIZER__PERCEPTIONMODEL_HPP_ diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 34c0d79e..96837eff 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 c7615292..2c2a4afc 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief AMCLLocalizer implementation (Bonxai + probabilistic inflation + ray casting). diff --git a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp index a40c68e9..7e9166a0 100644 --- a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 481d6830..bee15a05 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -75,4 +75,4 @@ This package does not create service servers or clients. ## License -GPL-3.0-only +Apache-2.0 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 7bff4232..9ba5ebab 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AMCLLocalizer method. diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index 93492165..16d61844 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple Localizer package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 693d006e..691ccb0c 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AMCLLocalizer class. diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index 731a256e..67475817 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_bonxai_maps_manager/README.md b/maps_managers/easynav_bonxai_maps_manager/README.md index 6f08a413..a01fb940 100644 --- a/maps_managers/easynav_bonxai_maps_manager/README.md +++ b/maps_managers/easynav_bonxai_maps_manager/README.md @@ -68,4 +68,4 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) ## License -GPL-3.0-only +Apache-2.0 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 8a81e0f1..4130fccb 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the BonxaiMapsManager method. diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 46c75db4..1ac676b4 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 bc8c3e4f..b5222471 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index d2df322a..ebdf66c9 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -162,4 +162,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 f4d0fb58..b36ee8ff 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the CostmapMapsManager method. 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 0ede2b6a..f4ba6b9a 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_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 940a2e18..4db115f0 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 186af843..6d0cd57a 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 229846b0..ea4e4f14 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include 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 b172a165..4c4343ff 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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 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 29433f3b..a9b26cf3 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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 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 0d05f16d..f14df242 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 @@ -1,8 +1,17 @@ // 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. +// 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 diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 2f223f28..9f925d2f 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -132,4 +132,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 6f7c2c1b..ed3b393c 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the NavMapMapsManager method. 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 74f1725a..70f010b9 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 a561ac73..e245681e 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ 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 d4c87015..ee342bca 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index 6951d89d..6b29d112 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 c230cb4b..aa0d04e4 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include 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 139278ba..47e9a95e 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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 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 cddda7ba..ad45c602 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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 #include 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 9de01f7e..d06b8904 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 @@ -1,8 +1,17 @@ // 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. +// 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 diff --git a/maps_managers/easynav_octomap_maps_manager/README.md b/maps_managers/easynav_octomap_maps_manager/README.md index 5d60468d..119b30f2 100644 --- a/maps_managers/easynav_octomap_maps_manager/README.md +++ b/maps_managers/easynav_octomap_maps_manager/README.md @@ -105,4 +105,4 @@ maps_manager_node: ## License -GPL-3.0-only +Apache-2.0 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 8b4f79ee..8d84e1b1 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the OctomapMapsManager method. 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 c0524721..f3477aa0 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 571433c3..e43778d1 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ 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 efa8a984..340859aa 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 6f4dae23..7e582c3f 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 a3fd6aad..e8e1a6c6 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 ff276efb..0f7165e0 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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 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 b25012c6..1ea4a111 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 @@ -1,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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 diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md index 30bb391e..1bf0af58 100644 --- a/maps_managers/easynav_routes_maps_manager/README.md +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -152,4 +152,4 @@ necessary TF tree. ## License -GPL-3.0-only +Apache-2.0 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 192af482..d194e338 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ #define 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 37fb17f3..e7d56c86 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the RoutesMapsManager class and related types. 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 c910a916..7ea65635 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef 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/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index 2bcb255a..f824ed9b 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Routes MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 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 index 2601842e..d4520234 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,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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_routes_maps_manager/RoutesMapsManager.hpp" #include "easynav_common/RTTFBuffer.hpp" 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 83908e19..b0682a91 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,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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_routes_maps_manager/filters/RoutesCostmapFilter.hpp" 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 40d8c72b..a6f574ca 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,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 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 665a25a7..889b8e7c 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,22 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . - +// 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 #include diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 4590daf1..10f59ab4 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -67,4 +67,4 @@ This manager does not broadcast TF; messages are stamped directly with their fra ## License -GPL-3.0-only +Apache-2.0 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 bd9a2037..b91a0186 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimpleMapsManager method. diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 15095e77..ed304e0c 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple MapsManager package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 411cdf8d..efd583d1 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the SimpleMapsManager class. 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 5e5b1006..6faec033 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 @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index d21e367c..6750fca3 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -110,4 +110,4 @@ This plugin does not create subscriptions or services directly; it reads inputs --- ## License -GPL-3.0-only +Apache-2.0 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 0b13806b..67de89db 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the CostmapPlanner class implementing A* path planning using Costmap2D. diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 053b985f..8d4eaa2c 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Costmap planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 2f9b356c..4062a6d8 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the CostmapPlanner class using A* on Costmap2D. diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 8e48b74e..6f5f891e 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -76,4 +76,4 @@ This plugin does not create subscriptions or services directly; it retrieves all The planner assumes frame consistency between NavMap, robot pose, and goals. No TF lookups are performed internally. ## License -GPL-3.0-only +Apache-2.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 ce01f50c..3b0cc064 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the AStarPlanner class implementing A* path planning using ::navmap::NavMap. diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 3376dd9e..78db89d5 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: navmap planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 0f1e7118..3f65c574 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Implementation of the AStarPlanner class using A* on ::navmap::NavMap (triangle graph). diff --git a/planners/easynav_simple_planner/README.md b/planners/easynav_simple_planner/README.md index 98335ace..324dc632 100644 --- a/planners/easynav_simple_planner/README.md +++ b/planners/easynav_simple_planner/README.md @@ -56,4 +56,4 @@ This plugin does not create subscriptions or services directly; it reads inputs This plugin does not perform TF lookups directly; frame consistency is assumed between map, robot pose, and published path. ## License -GPL-3.0-only +Apache-2.0 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 48bda79e..985f782f 100644 --- a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp +++ b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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. /// \file /// \brief Declaration of the SimplePlanner class implementing A* path planning. diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index e092f111..f40d8b64 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -5,7 +5,7 @@ 0.0.2 Easy Navigation: Simple planner package. Francisco Martín Rico - GPL-3.0-only + Apache-2.0 ament_cmake 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 7d116d20..3e696190 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -1,21 +1,17 @@ // 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. +// 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 // -// 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. +// http://www.apache.org/licenses/LICENSE-2.0 // -// 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 . +// 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 #include From d7cf00bbdb2da71f7f19211f45d1c1aa125729c4 Mon Sep 17 00:00:00 2001 From: migueldm Date: Mon, 23 Feb 2026 12:49:48 +0100 Subject: [PATCH 155/230] Call set pose srvc for first pose for fastest convergence. Updated params to stabilice the filter in summit --- .../config/example_params_summit_dual.yaml | 31 ++++++++++++------- .../FusionLocalizer.hpp | 2 ++ .../FusionLocalizer.cpp | 29 ++++++++++------- 3 files changed, 39 insertions(+), 23 deletions(-) diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml index 52e7b06e..229f09c2 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -46,15 +46,15 @@ localizer_node: odom0_differential: false # --- INPUT 3: IMU --- - imu0: /imu/data - imu0_config: - [false, false, false, - false, false, false, - true, true, true, - true, true, true, - false, false, false] - imu0_differential: false - imu0_remove_gravitational_acceleration: true + # imu0: /imu/data + # imu0_config: + # [false, false, false, + # false, false, false, + # false, false, false, + # false, false, true, + # true, true, true] + # imu0_differential: false + # imu0_remove_gravitational_acceleration: true # twist0: /gps/fix_velocity # twist0_config: @@ -84,6 +84,13 @@ localizer_node: global_filter: two_d_mode: true publish_tf: true + debug: true + debug_out_file: /tmp/ukf_global_debug.txt + transform_timeout: 0.1 + smooth_lagged_data: true + # Defines the time window in seconds to keep past states in memory. + history_length: 5.0 + dynamic_process_noise_covariance: true # --- Reference Frames --- map_frame: map @@ -95,7 +102,7 @@ localizer_node: gps0: /gps/fix gps0_config: [true, true, false, - false, false, true, + false, false, false, false, false, false, false, false, false, false, false, false] @@ -116,8 +123,8 @@ localizer_node: imu0_config: [false, false, false, false, false, false, - true, true, true, - true, true, true, + false, false, false, + false, false, true, false, false, false] imu0_differential: false imu0_remove_gravitational_acceleration: true 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 330869b2..19f829b6 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -82,6 +82,8 @@ class FusionLocalizer : public easynav::LocalizerMethodBase rclcpp::Publisher::SharedPtr gps_debug_pub_{nullptr}; std::string navsatfix_topic_; + bool first_pose_received_{false}; + }; } // namespace easynav 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 147ce87b..7fe7e41c 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -91,10 +91,16 @@ void FusionLocalizer::update_rt(NavState & nav_state) if (gps_time > last_gps_stamp_[i]) { EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); last_gps_stamp_[i] = gps_time; - auto pose = navsatfix_to_pose(gps_data[i]->data); - gps_debug_pub_->publish(pose); + auto pose = std::make_shared(navsatfix_to_pose(gps_data[i]->data)); + if (!first_pose_received_) { + RCLCPP_INFO(get_node()->get_logger(), "First valid GPS fix received. Initializing filter state."); + ukf_global_->setPoseCallback(pose); + first_pose_received_ = true; + continue; + } + gps_debug_pub_->publish(*pose); ukf_global_->poseCallback( - std::make_shared(pose), + pose, gps_cb_arr[i], tf_info.map_frame, gps_data[i]->data.header.frame_id, @@ -176,6 +182,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.pose.pose.orientation.w = 1.0; pose_msg.pose.covariance.fill(0.0); + + double default_var = 1.0; // 1 meter variance standard if (navsat_msg.position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) { @@ -183,20 +191,19 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4]; pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; - pose_msg.pose.covariance[21] = kNoOrientationCovariance; - pose_msg.pose.covariance[28] = kNoOrientationCovariance; - pose_msg.pose.covariance[35] = kNoOrientationCovariance; + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; } else { // Fallback variances if GPS doesn't provide them - double default_var = 1.0; // 1 meter variance standard pose_msg.pose.covariance[0] = default_var; pose_msg.pose.covariance[7] = default_var; pose_msg.pose.covariance[14] = default_var; - pose_msg.pose.covariance[21] = kNoOrientationCovariance; - pose_msg.pose.covariance[28] = kNoOrientationCovariance; - pose_msg.pose.covariance[35] = kNoOrientationCovariance; - + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, "NavSatFix covariance type unknown or invalid. Using default covariance."); } From 8b2d7f7ce7bae062a12ac8c518a7f17009cc03ec Mon Sep 17 00:00:00 2001 From: migueldm Date: Mon, 23 Feb 2026 15:36:15 +0100 Subject: [PATCH 156/230] Uncrustify changes --- .../FusionLocalizer.cpp | 29 +++++++++++-------- .../easynav_fusion_localizer/ukf_wrapper.cpp | 1 - 2 files changed, 17 insertions(+), 13 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 7fe7e41c..99605305 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -53,7 +53,8 @@ void FusionLocalizer::on_initialize() localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); navsat_pub_ = localizer_node->create_publisher( navsatfix_topic_, rclcpp::QoS(10)); - gps_debug_pub_ = localizer_node->create_publisher( + gps_debug_pub_ = + localizer_node->create_publisher( "gps_pose", rclcpp::QoS(10)); } catch (const std::exception & e) { RCLCPP_FATAL( @@ -91,9 +92,12 @@ void FusionLocalizer::update_rt(NavState & nav_state) if (gps_time > last_gps_stamp_[i]) { EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); last_gps_stamp_[i] = gps_time; - auto pose = std::make_shared(navsatfix_to_pose(gps_data[i]->data)); + auto pose = + std::make_shared(navsatfix_to_pose( + gps_data[i]->data)); if (!first_pose_received_) { - RCLCPP_INFO(get_node()->get_logger(), "First valid GPS fix received. Initializing filter state."); + RCLCPP_INFO(get_node()->get_logger(), + "First valid GPS fix received. Initializing filter state."); ukf_global_->setPoseCallback(pose); first_pose_received_ = true; continue; @@ -182,10 +186,11 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.pose.pose.orientation.w = 1.0; pose_msg.pose.covariance.fill(0.0); - + double default_var = 1.0; // 1 meter variance standard - if (navsat_msg.position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) + if (navsat_msg.position_covariance_type == + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) { pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0]; pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4]; @@ -196,15 +201,15 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.pose.covariance[35] = default_var; } else { // Fallback variances if GPS doesn't provide them - pose_msg.pose.covariance[0] = default_var; - pose_msg.pose.covariance[7] = default_var; - pose_msg.pose.covariance[14] = default_var; + pose_msg.pose.covariance[0] = default_var; + pose_msg.pose.covariance[7] = default_var; + pose_msg.pose.covariance[14] = default_var; - pose_msg.pose.covariance[21] = default_var; - pose_msg.pose.covariance[28] = default_var; - pose_msg.pose.covariance[35] = default_var; + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, "NavSatFix covariance type unknown or invalid. Using default covariance."); } 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 20e6ef05..6164b4de 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 @@ -2101,7 +2101,6 @@ void UkfWrapper::odometryCallback( 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); From 350bb8bc5d551ab471ddacc4bc9081fd570e0d82 Mon Sep 17 00:00:00 2001 From: migueldm Date: Tue, 24 Feb 2026 09:27:58 +0100 Subject: [PATCH 157/230] Initialization considers IMU orientation --- .../config/example_params_summit_dual.yaml | 2 +- .../FusionLocalizer.cpp | 28 ++++--------------- 2 files changed, 7 insertions(+), 23 deletions(-) diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml index 229f09c2..3e5d143c 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -89,7 +89,7 @@ localizer_node: transform_timeout: 0.1 smooth_lagged_data: true # Defines the time window in seconds to keep past states in memory. - history_length: 5.0 + # history_length: 5.0 dynamic_process_noise_covariance: true # --- Reference Frames --- 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 99605305..825c6a09 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -78,7 +78,6 @@ void FusionLocalizer::on_initialize() void FusionLocalizer::update_rt(NavState & nav_state) { - EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_update_rt"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (n_gps_sensors_ && nav_state.has("gnss")) { @@ -98,6 +97,12 @@ void FusionLocalizer::update_rt(NavState & nav_state) if (!first_pose_received_) { RCLCPP_INFO(get_node()->get_logger(), "First valid GPS fix received. Initializing filter state."); + if(nav_state.has("imu")) { + auto imu_data = nav_state.get(std::string("imu")); + if (!imu_data.empty()) { + pose->pose.pose.orientation = imu_data[0]->data.orientation; + } + } ukf_global_->setPoseCallback(pose); first_pose_received_ = true; continue; @@ -132,27 +137,6 @@ void FusionLocalizer::update([[maybe_unused]] NavState & nav_state) } -/** - * @brief Converts a NavSatFix message to a PoseWithCovarianceStamped in UTM coordinates. - * - * This function transforms the latitude, longitude, and altitude from a sensor_msgs::msg::NavSatFix - * message into UTM coordinates and populates a geometry_msgs::msg::PoseWithCovarianceStamped message. - * The output pose is expressed relative to a predefined UTM origin. The orientation is set to identity - * (no rotation), as orientation cannot be inferred from GPS data alone. - * - * Covariance handling: - * - If the NavSatFix message provides a diagonal covariance (COVARIANCE_TYPE_DIAGONAL_KNOWN), - * the corresponding variances are copied into the pose covariance matrix. - * - If not, a default variance of 1.0 is used for the position, and a warning is issued. - * - * @param navsat_msg The input NavSatFix message containing latitude, longitude, altitude, and covariance. - * @return geometry_msgs::msg::PoseWithCovarianceStamped The resulting pose in UTM coordinates with covariance. - * - * @note - * The function uses RCLCPP_WARN_THROTTLE to log a warning if the covariance type is unknown or invalid. - * This throttled warning will only be emitted at most once every 5 seconds, preventing log flooding, - * unlike RCLCPP_WARN which would log a warning every time the condition is met. - */ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose( const sensor_msgs::msg::NavSatFix & navsat_msg) { From 34aef9fcb1ccf706550b9ac5ac5daf91e940bd78 Mon Sep 17 00:00:00 2001 From: migueldm Date: Wed, 25 Feb 2026 17:26:22 +0100 Subject: [PATCH 158/230] Added simple configuration to launch full demo --- .../easynav_fusion_localizer/CMakeLists.txt | 4 ++ .../config/example_params_summit_dual.yaml | 56 ++++++++++++++++-- .../easynav_fusion_localizer/maps/empty.png | Bin 0 -> 4849 bytes .../easynav_fusion_localizer/maps/empty.yaml | 7 +++ .../easynav_fusion_localizer/maps/square.png | Bin 0 -> 11002 bytes .../easynav_fusion_localizer/maps/square.yaml | 7 +++ 6 files changed, 70 insertions(+), 4 deletions(-) create mode 100644 localizers/easynav_fusion_localizer/maps/empty.png create mode 100644 localizers/easynav_fusion_localizer/maps/empty.yaml create mode 100644 localizers/easynav_fusion_localizer/maps/square.png create mode 100644 localizers/easynav_fusion_localizer/maps/square.yaml diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index 4db3bed8..94db9550 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -72,6 +72,10 @@ install(TARGETS RUNTIME DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY maps/ + DESTINATION share/${PROJECT_NAME}/maps +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml index 3e5d143c..482386e8 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -1,12 +1,25 @@ controller_node: ros__parameters: use_sim_time: true - controller_types: [dummy] + controller_types: [mppi] dummy: rt_freq: 30.0 plugin: easynav_controller/DummyController cycle_time_nort: 0.01 cycle_time_rt: 0.0001 + mppi: + rt_freq: 30.0 + plugin: easynav_mppi_controller/MPPIController + num_samples: 200 + horizon_steps: 5 + dt: 0.15 + lambda: 1.0 + fov: 1.57 + max_linear_velocity: 1.5 + max_angular_velocity: 1.0 + max_linear_acceleration: 0.5 + max_angular_acceleration: 0.5 + safety_radius: 0.3 localizer_node: ros__parameters: @@ -165,7 +178,32 @@ localizer_node: maps_manager_node: ros__parameters: use_sim_time: true - map_types: [dummy] + map_types: [costmap] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + filters: [routes_costmap] + package: easynav_fusion_localizer + map_path_file: routes/simple_square.yaml + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 50 + route_width: 0.5 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easynav_fusion_localizer + map_path_file: maps/square.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 dummy: freq: 10.0 plugin: easynav_maps_manager/DummyMapsManager @@ -175,18 +213,23 @@ maps_manager_node: planner_node: ros__parameters: use_sim_time: true - planner_types: [dummy] + planner_types: [simple] dummy: freq: 1.0 plugin: easynav_planner/DummyPlanner cycle_time_nort: 0.2 cycle_time_rt: 0.0001 + simple: + freq: 10.0 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true sensors_node: ros__parameters: use_sim_time: true forget_time: 0.5 - sensors: [imu, gps] + sensors: [imu, gps, laser1] perception_default_frame: odom imu: topic: /imu/data @@ -194,6 +237,11 @@ sensors_node: gps: topic: /gps/fix type: sensor_msgs/msg/NavSatFix + laser1: + topic: /front_laser/points + type: sensor_msgs/msg/PointCloud2 + group: points + system_node: ros__parameters: diff --git a/localizers/easynav_fusion_localizer/maps/empty.png b/localizers/easynav_fusion_localizer/maps/empty.png new file mode 100644 index 0000000000000000000000000000000000000000..cd6eb69a9179a64f6596a543a2ed9dbc40bb35fc GIT binary patch literal 4849 zcmeAS@N?(olHy`uVBq!ia0y~yV15C@985rwLkFEVGcYi=WI8(scse^P6cpvBW#*(Z zFlbDyow(oou!GFe_+ZzGT}`rG7AY(I$^s|$ZRK)ZA&}J?ChMd3m(^Ell8Ctb!L1J- z^dDW-+`V}n-?}D+AIwMW9p;F{E;;gEwN=bePUic=yL*q{wRXq|o0;}ig~50GSsyLU zXP;Il-PdMuG3c>inHhO(j>^)9*8AQGK6oA5lm2u5?_!=+tlf-^6D(rok0%0~tpBVHvf?T);8r6av#@v3FP z`zCyjc)I6k2V|b=O&QPSLMP$9B zB?_pvgB2j~j0_V%DHTv+VzfkoH5HJP6RnftXo*6Fh89XX9xYL@rDGzBMG~v`(GmsP zfxum)4oFfQEm1~G6fBK2!f6pD9TV;_jFu=^8(N@nh6f(uzAM(|a>H!ah6Jv&ReOqb PL4ziqu6{1-oD!M`cqa44*@+DSSXO-M)r2{>TH=RKZ- zI)W&x!YB&rIEn`fjv_h^qRZ~W0BXEPxuT2;D!5;F5``Uo`y6%u+3+5rySl2rs$c!; ztFOb#i0MHC?Op952pSj?91saXHoxM(eeHmRzv~}&z#l_ebgViO)xZj+Tr81dusT(N z!I)Meh9GVInlERqwH?#<<+C9wTiZbH+OBU!@`IH(B9qe6HijJi(>xD}2W?xKU$x;^ z{ZC`=M*cIpU-0IyM`VT$ZZK^4yTK-5Y|_*7%WJKcJiPkzg}R(vy;D$i)$(&OWv)dq zE1;7%ud1``brZx|43tTO0zoX)PxYU$~7!Ad|LjV zF~!sN&fO6-{CVRqQ+Ig}e@>clI@Be*Bg?~ooXL1&KW_3J7Mm#jWq>`yEX9ER(^xNG~7)b7&q2;+;-$a z=DBsVXL@Jco~?+}Z$%lo+2b4ruU^WUQUC2w{u*sIXXkw0-Tp4?VlL0xc0{?!tL@?( z=bs*ozcl+h?kd}1hivihN#Zcszyxhb?UWB9-WhPJ;m3tbjMXTw)o5}_}INC|7xs!V6(oUcq6;~%oyaadCgB8 z+FB)TEjJid-5Jo7cI7vzS6602kfW~zkTI4&jVF}LD5yv-z$jXo0F&R)*fmLa}mJABvN4&PcAxMzO z6oeH3yO*X~B7Or_uh{S>L^?eK0p_p$_R==zPACH{KA#sL7bfH3g#`GJ@b!5jxlkhF z5tkeRmrLWaxMVidOF(8K7>z8TalFVZwur$6)IeAqMh{mZi7GXkD8z7Az&S+%{9ppM z2oo~dWHt+YFcATrEMU_ZWR6(CMQL;qSIG40;UZiq0jxx&J)^>1iGVAH0Oc^nVg{MU zpb5!L4qym}a;XmqcLC#;uOIV4JBd&=8i1-X;1r>= zcnF18V zm0*}?3LYvnrl3MoFcA?a@MU}?oQEbRU|{z!(bX)MyoCxhp@7N8ONW9|k(nYkMiu}O zS;Xc7sesEwX=rcnD!Ev#L6z9#1b`#J3ec0l3icoh<=I|O7baxOda(pmw;i*BJYK-1uhF*?}fm7DO=cOf$O~xcrRrO zyFMCR_HSN_u|#l|)qq#tddD*q@Vaa#2nz}zUYwy$|C5WrRX;^=oC<;{?)a~jHs3D= z2>Yl*_cCjFzg&jDTS3H;9}~eEiFzK4ANgl4b{Gyp&Z|QL zCP!=QTdvVoHP*QS!p3q~%<&V_e@^uO)3LPb*=1Yk<$>gQP5a?#HAB*GRC-tY zb!DT@x757j3aI>M{)*TtsC<-7dM!22)5Qv^%L;0I`vC7)UEK6Fv4>tLlDL8+^Se)? z`rLj{$AQ8^R#%2+ep%PE;->CLHpJ_W%ydbs3t+ZrOs3$pDAAYAK_I-y6WlLnt1#yC2fPss5hscn)_5RrNrA*zL!v{<3qms-pPLcmENf_)(frHv)0)LrI7YMymm-^g zxtE$k+!D#^cE-eDV3IC&n3O1MaJ9m{ub&lPJC%4(Q4BbXI9ox-Rujdodo15lR$6+l zrY34bLbzuqW(BqWIMVd?lC|ef*%Al5PMoisp3FD7+;XXBDU6sV6R`tDj~dR}<(+Xk z1xaU!0JdJff7poRaUVZFfRMT6b^dz`f9i{;t81)3>SYZb?C(jM!GHjBPPy&QTIKU> z!(}_jZ8cz1GwV02w;Wq_!uC-hP=JBMyLe&@?sS2KZv76n8Tmx$9XrsYb+W5Y*M2{t zqLn#d$(&($(24=IO-Jeu=T09t+!-&p+u6HH8c{Cb!aQJ}&UFhi5;h|!CMGBSKzpY& z0@B@BPZD_9lnX~x(jK0;esAg6eE;0^_*y}=9nL9-dtMv90PWFw*1Z+--cJNUP>tx{ zUdI5Bn$4qGecqgqp{VKLC$??aj1PCW1Be#v^dV zvNJ?^_0H#$COnmXS99?*JG_dO3f~=|s~&KL1?%26}DSsFpss|3F^7d+RKpdPf|lx=jr&Vw2IO?*Z-c zMmt$p2>3uW+EEz@1?XKUDsEctT`~^5z=K*&)L$et%N@KQeq)1cnrQ-3CVJ8^Y%^x8 zK3vkXGxF;W^K0%{+u&P{vgyTj6T$DLvIqilmL<8F7HlE*3%oNM+S^Sn(pZfQ08QFT z2|PF7PFH!PxS@0a(F#}bR`>?A0ue0uG@TH2koC<9fmQd(_aj0_kPt;LQI~P$M?HB9_?Cf2Q^P$)pyZ| zKKjKio#DIr_<)j+>b}hZQ{vUj3i{YG4;&(Wl1)0Bx)~&{wYk2;!BAd0e1tIyevJ`$ zuy(JV*fjrvP&c|)NeumKn8q!-aVFTdI+hYcqI9L^&yaSuXa1ZFer@tqX4R{%?83sG zvIf$UMcWgMLBM4-52AmL8`b4E0@9_2IUUNzUqc>0#y1Mfmo!Eq_ack=Tb4f}1Qauy zTaL4)jp2g Date: Thu, 26 Feb 2026 18:40:27 +0100 Subject: [PATCH 159/230] update changelogs Signed-off-by: Juan S. Cely G. --- .../easynav_mpc_controller/CHANGELOG.rst | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 controllers/easynav_mpc_controller/CHANGELOG.rst diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst new file mode 100644 index 00000000..99bbd74b --- /dev/null +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -0,0 +1,23 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_mpc_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.0 (2026-02-26) +------------------ +* GPLv3 -> Apache 2.0 +* Collision checker scope was changed +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Collision avoidance was improved +* Trim path for MPC +* Obstacles are detected +* MPC with differential model is working +* MPC Controller minimizes path following +* Visualization Marker was added +* MPC controller is working +* MPC was added +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely, Juan S. Cely G., Miguel + +0.0.2 (2025-10-12) +------------------ From a24853dfdce3674f9f2e3dc2f82e2e0ecaec05f9 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 26 Feb 2026 18:41:47 +0100 Subject: [PATCH 160/230] update changelogs Signed-off-by: Juan S. Cely G. --- controllers/easynav_mpc_controller/CHANGELOG.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst index 99bbd74b..7abb1b3e 100644 --- a/controllers/easynav_mpc_controller/CHANGELOG.rst +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_mpc_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.2.0 (2026-02-26) ------------------- +Forthcoming +----------- * GPLv3 -> Apache 2.0 * Collision checker scope was changed * Remove C++20/C++23 features and update to new MethodBase interface From 2b660f29e9ba3cd79b0ef2ee4fe184d9378be064 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 26 Feb 2026 18:52:44 +0100 Subject: [PATCH 161/230] 0.2.0 Signed-off-by: Juan S. Cely G. --- common/easynav_costmap_common/package.xml | 2 +- common/easynav_simple_common/package.xml | 2 +- controllers/easynav_mpc_controller/package.xml | 2 +- controllers/easynav_mppi_controller/package.xml | 2 +- controllers/easynav_serest_controller/package.xml | 2 +- controllers/easynav_simple_controller/package.xml | 2 +- controllers/easynav_vff_controller/package.xml | 2 +- localizers/easynav_costmap_localizer/package.xml | 2 +- localizers/easynav_fusion_localizer/package.xml | 2 +- localizers/easynav_gps_localizer/package.xml | 2 +- localizers/easynav_navmap_localizer/package.xml | 2 +- localizers/easynav_simple_localizer/package.xml | 2 +- maps_managers/easynav_bonxai_maps_manager/package.xml | 2 +- maps_managers/easynav_costmap_maps_manager/package.xml | 2 +- maps_managers/easynav_navmap_maps_manager/package.xml | 2 +- maps_managers/easynav_octomap_maps_manager/package.xml | 2 +- maps_managers/easynav_routes_maps_manager/package.xml | 2 +- maps_managers/easynav_simple_maps_manager/package.xml | 2 +- planners/easynav_costmap_planner/package.xml | 2 +- planners/easynav_navmap_planner/package.xml | 2 +- planners/easynav_simple_planner/package.xml | 2 +- 21 files changed, 21 insertions(+), 21 deletions(-) diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 7f3c271f..8094db30 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -2,7 +2,7 @@ easynav_costmap_common - 0.0.2 + 0.2.0 Easy Navigation: Simple Common package. Francisco Martín Rico diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index 38ac4fb7..ea5d94d6 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -2,7 +2,7 @@ easynav_simple_common - 0.0.2 + 0.2.0 Easy Navigation: Simple Common package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 6658226d..91e39ec7 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -2,7 +2,7 @@ easynav_mpc_controller - 0.0.1 + 0.2.0 Easy Navigation: MPC Controller package. Juan S. Cely G. Apache-2.0 diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index c3382a3c..1a5e4a5e 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -2,7 +2,7 @@ easynav_mppi_controller - 0.0.2 + 0.2.0 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index a57476ed..30d1414d 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -2,7 +2,7 @@ easynav_serest_controller - 0.0.2 + 0.2.0 Easy Navigation: SeReST Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 99181b8b..e80cacea 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -2,7 +2,7 @@ easynav_simple_controller - 0.0.2 + 0.2.0 Easy Navigation: Simple Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index 07b23911..5d3ad153 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -2,7 +2,7 @@ easynav_vff_controller - 0.0.2 + 0.2.0 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index a8d85e0b..f7aebdf4 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_costmap_localizer - 0.0.2 + 0.2.0 Easy Navigation: Costmap Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 6d2175c6..783ecf53 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -2,7 +2,7 @@ easynav_fusion_localizer - 0.0.2 + 0.2.0 Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index 13d3d8d7..b70cf111 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -2,7 +2,7 @@ easynav_gps_localizer - 0.0.2 + 0.2.0 Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 96837eff..3f494984 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_navmap_localizer - 0.0.2 + 0.2.0 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index 16d61844..f6ef81ba 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -2,7 +2,7 @@ easynav_simple_localizer - 0.0.2 + 0.2.0 Easy Navigation: Simple Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 1ac676b4..847ff1be 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_bonxai_maps_manager - 0.0.2 + 0.2.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 6d0cd57a..6dbf5bdf 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_costmap_maps_manager - 0.0.2 + 0.2.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index 6b29d112..b49608be 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_navmap_maps_manager - 0.0.2 + 0.2.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 7e582c3f..a5f2c18a 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_octomap_maps_manager - 0.0.2 + 0.2.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index f824ed9b..b3545fa0 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_routes_maps_manager - 0.0.2 + 0.2.0 Easy Navigation: Routes MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index ed304e0c..97c372dd 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_simple_maps_manager - 0.0.2 + 0.2.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 8d4eaa2c..622e215b 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_costmap_planner - 0.0.2 + 0.2.0 Easy Navigation: Costmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 78db89d5..0356042c 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_navmap_planner - 0.0.2 + 0.2.0 Easy Navigation: navmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index f40d8b64..87fe4200 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -2,7 +2,7 @@ easynav_simple_planner - 0.0.2 + 0.2.0 Easy Navigation: Simple planner package. Francisco Martín Rico Apache-2.0 From 9469085aa3602172edd3c3a5979d7c5ba1c0ddcb Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 26 Feb 2026 19:07:27 +0100 Subject: [PATCH 162/230] update changelogs Signed-off-by: Juan S. Cely G. --- common/easynav_costmap_common/CHANGELOG.rst | 8 ++++++++ common/easynav_simple_common/CHANGELOG.rst | 10 ++++++++++ .../easynav_mpc_controller/CHANGELOG.rst | 3 ++- .../easynav_mppi_controller/CHANGELOG.rst | 14 ++++++++++++++ .../easynav_serest_controller/CHANGELOG.rst | 18 ++++++++++++++++++ .../easynav_simple_controller/CHANGELOG.rst | 15 +++++++++++++++ .../easynav_vff_controller/CHANGELOG.rst | 15 +++++++++++++++ .../easynav_costmap_localizer/CHANGELOG.rst | 15 +++++++++++++++ localizers/easynav_gps_localizer/CHANGELOG.rst | 15 +++++++++++++++ .../easynav_simple_localizer/CHANGELOG.rst | 15 +++++++++++++++ .../easynav_bonxai_maps_manager/CHANGELOG.rst | 15 +++++++++++++++ .../easynav_costmap_maps_manager/CHANGELOG.rst | 15 +++++++++++++++ .../easynav_navmap_maps_manager/CHANGELOG.rst | 15 +++++++++++++++ .../easynav_octomap_maps_manager/CHANGELOG.rst | 15 +++++++++++++++ .../easynav_routes_maps_manager/CHANGELOG.rst | 17 +++++++++++++++++ .../easynav_simple_maps_manager/CHANGELOG.rst | 15 +++++++++++++++ planners/easynav_costmap_planner/CHANGELOG.rst | 15 +++++++++++++++ planners/easynav_navmap_planner/CHANGELOG.rst | 15 +++++++++++++++ planners/easynav_simple_planner/CHANGELOG.rst | 15 +++++++++++++++ 19 files changed, 264 insertions(+), 1 deletion(-) diff --git a/common/easynav_costmap_common/CHANGELOG.rst b/common/easynav_costmap_common/CHANGELOG.rst index 924b909c..3006005d 100644 --- a/common/easynav_costmap_common/CHANGELOG.rst +++ b/common/easynav_costmap_common/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package easynav_costmap_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Remove C++20/C++23 features and update to new MethodBase interface +* Cleanup unused headers +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/common/easynav_simple_common/CHANGELOG.rst b/common/easynav_simple_common/CHANGELOG.rst index 7d5ee0d0..91e0c825 100644 --- a/common/easynav_simple_common/CHANGELOG.rst +++ b/common/easynav_simple_common/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package easynav_simple_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Rolling updates +* TFInfo in RTTFBuffer +* Fix downsample in Simple +* Cleanup unused headers +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst index 7abb1b3e..92145a81 100644 --- a/controllers/easynav_mpc_controller/CHANGELOG.rst +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -4,6 +4,7 @@ Changelog for package easynav_mpc_controller Forthcoming ----------- +* 0.2.0 * GPLv3 -> Apache 2.0 * Collision checker scope was changed * Remove C++20/C++23 features and update to new MethodBase interface @@ -17,7 +18,7 @@ Forthcoming * Visualization Marker was added * MPC controller is working * MPC was added -* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely, Juan S. Cely G., Miguel +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely G., Miguel 0.0.2 (2025-10-12) ------------------ diff --git a/controllers/easynav_mppi_controller/CHANGELOG.rst b/controllers/easynav_mppi_controller/CHANGELOG.rst index 7c4dda88..f5b66d5c 100644 --- a/controllers/easynav_mppi_controller/CHANGELOG.rst +++ b/controllers/easynav_mppi_controller/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package easynav_mppi_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Remove C++20/C++23 features and update to new MethodBase interface +* Merge branch 'set_robot_frame' into frames-fix-pr-40 +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Include package in cmake and package.xml +* Fix link error when running MPPI in Realease +* Stop controllers at IDLE +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely G., Miguel, estherag + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/controllers/easynav_serest_controller/CHANGELOG.rst b/controllers/easynav_serest_controller/CHANGELOG.rst index 0c86bc2a..616eb605 100644 --- a/controllers/easynav_serest_controller/CHANGELOG.rst +++ b/controllers/easynav_serest_controller/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package easynav_serest_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Remove C++20/C++23 features and update to new MethodBase interface +* Merge branch 'set_robot_frame' into frames-fix-pr-40 +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Sync to current EasyNavigation +* Translate comments +* Referencing base class if ot void +* Merge remote-tracking branch 'upstream/rolling' into rolling +* Fix Link error in controller +* Optimize execution +* Cleanup unused headers +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely G., Miguel, estherag + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/controllers/easynav_simple_controller/CHANGELOG.rst b/controllers/easynav_simple_controller/CHANGELOG.rst index d4d6ebc3..eff04f45 100644 --- a/controllers/easynav_simple_controller/CHANGELOG.rst +++ b/controllers/easynav_simple_controller/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_simple_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, estherag + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/controllers/easynav_vff_controller/CHANGELOG.rst b/controllers/easynav_vff_controller/CHANGELOG.rst index 6f683634..8c6ba653 100644 --- a/controllers/easynav_vff_controller/CHANGELOG.rst +++ b/controllers/easynav_vff_controller/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_vff_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/localizers/easynav_costmap_localizer/CHANGELOG.rst b/localizers/easynav_costmap_localizer/CHANGELOG.rst index d0592d8f..425bdb64 100644 --- a/localizers/easynav_costmap_localizer/CHANGELOG.rst +++ b/localizers/easynav_costmap_localizer/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_costmap_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/localizers/easynav_gps_localizer/CHANGELOG.rst b/localizers/easynav_gps_localizer/CHANGELOG.rst index d08e6290..d38c68ce 100644 --- a/localizers/easynav_gps_localizer/CHANGELOG.rst +++ b/localizers/easynav_gps_localizer/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_gps_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/localizers/easynav_simple_localizer/CHANGELOG.rst b/localizers/easynav_simple_localizer/CHANGELOG.rst index 22209583..a41bb621 100644 --- a/localizers/easynav_simple_localizer/CHANGELOG.rst +++ b/localizers/easynav_simple_localizer/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_simple_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst index 77e5495a..a96993f2 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_bonxai_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst index 7e45f896..744f4ca0 100644 --- a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_costmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Jose Miguel, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst index 0e2272bd..cf9fa781 100644 --- a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_navmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst index d30661e8..f1ed5768 100644 --- a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_octomap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst index bc658d4e..ddc80c96 100644 --- a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Final adjustements +* Refactor to use TFInfo +* Added missing dep +* Added missing licenses +* Tests and README +* Route Maps Manager with filters. Costmap filter +* Routes Maps manager finalized +* Add/Remove segment +* Interactive markers and ids in the segments +* Basic route server +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst index bc658d4e..ae4428f1 100644 --- a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/planners/easynav_costmap_planner/CHANGELOG.rst b/planners/easynav_costmap_planner/CHANGELOG.rst index 17d80b4d..59184072 100644 --- a/planners/easynav_costmap_planner/CHANGELOG.rst +++ b/planners/easynav_costmap_planner/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_costmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Jose Miguel, José Miguel Guerrero, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/planners/easynav_navmap_planner/CHANGELOG.rst b/planners/easynav_navmap_planner/CHANGELOG.rst index bb10c0b2..c9603fa6 100644 --- a/planners/easynav_navmap_planner/CHANGELOG.rst +++ b/planners/easynav_navmap_planner/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_navmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/planners/easynav_simple_planner/CHANGELOG.rst b/planners/easynav_simple_planner/CHANGELOG.rst index 472ae955..8980db4c 100644 --- a/planners/easynav_simple_planner/CHANGELOG.rst +++ b/planners/easynav_simple_planner/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_simple_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 0.2.0 +* GPLv3 -> Apache 2.0 +* Documentation was corrected +* Add a base_footprint frame in TFInfo +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Referencing base class if ot void +* Optimize execution +* Cleanup unused headers +* Update sheets +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel + 0.0.2 (2025-10-12) ------------------ * Reorganization initial From 3ceec0b0508effdc9933562aa5de1b1c0307f3ec Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 27 Feb 2026 14:48:02 +0100 Subject: [PATCH 163/230] 0.2.1 --- common/easynav_costmap_common/CHANGELOG.rst | 4 ++-- common/easynav_costmap_common/package.xml | 2 +- common/easynav_simple_common/CHANGELOG.rst | 4 ++-- common/easynav_simple_common/package.xml | 2 +- controllers/easynav_mpc_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_mpc_controller/package.xml | 2 +- controllers/easynav_mppi_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_mppi_controller/package.xml | 2 +- controllers/easynav_serest_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_serest_controller/package.xml | 2 +- controllers/easynav_simple_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_simple_controller/package.xml | 2 +- controllers/easynav_vff_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_vff_controller/package.xml | 2 +- localizers/easynav_costmap_localizer/CHANGELOG.rst | 4 ++-- localizers/easynav_costmap_localizer/package.xml | 2 +- localizers/easynav_fusion_localizer/package.xml | 2 +- localizers/easynav_gps_localizer/CHANGELOG.rst | 4 ++-- localizers/easynav_gps_localizer/package.xml | 2 +- localizers/easynav_navmap_localizer/package.xml | 2 +- localizers/easynav_simple_localizer/CHANGELOG.rst | 4 ++-- localizers/easynav_simple_localizer/package.xml | 2 +- maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_bonxai_maps_manager/package.xml | 2 +- maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_costmap_maps_manager/package.xml | 2 +- maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_navmap_maps_manager/package.xml | 2 +- maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_octomap_maps_manager/package.xml | 2 +- maps_managers/easynav_routes_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_routes_maps_manager/package.xml | 2 +- maps_managers/easynav_simple_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_simple_maps_manager/package.xml | 2 +- planners/easynav_costmap_planner/CHANGELOG.rst | 4 ++-- planners/easynav_costmap_planner/package.xml | 2 +- planners/easynav_navmap_planner/CHANGELOG.rst | 4 ++-- planners/easynav_navmap_planner/package.xml | 2 +- planners/easynav_simple_planner/CHANGELOG.rst | 4 ++-- planners/easynav_simple_planner/package.xml | 2 +- 40 files changed, 59 insertions(+), 59 deletions(-) diff --git a/common/easynav_costmap_common/CHANGELOG.rst b/common/easynav_costmap_common/CHANGELOG.rst index 3006005d..4cb20109 100644 --- a/common/easynav_costmap_common/CHANGELOG.rst +++ b/common/easynav_costmap_common/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_costmap_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Remove C++20/C++23 features and update to new MethodBase interface diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 8094db30..16421c69 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -2,7 +2,7 @@ easynav_costmap_common - 0.2.0 + 0.2.1 Easy Navigation: Simple Common package. Francisco Martín Rico diff --git a/common/easynav_simple_common/CHANGELOG.rst b/common/easynav_simple_common/CHANGELOG.rst index 91e0c825..dda3a91f 100644 --- a/common/easynav_simple_common/CHANGELOG.rst +++ b/common/easynav_simple_common/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Rolling updates diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index ea5d94d6..b21e9543 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -2,7 +2,7 @@ easynav_simple_common - 0.2.0 + 0.2.1 Easy Navigation: Simple Common package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst index 92145a81..3d304e76 100644 --- a/controllers/easynav_mpc_controller/CHANGELOG.rst +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_mpc_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Collision checker scope was changed diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 91e39ec7..95223ee0 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -2,7 +2,7 @@ easynav_mpc_controller - 0.2.0 + 0.2.1 Easy Navigation: MPC Controller package. Juan S. Cely G. Apache-2.0 diff --git a/controllers/easynav_mppi_controller/CHANGELOG.rst b/controllers/easynav_mppi_controller/CHANGELOG.rst index f5b66d5c..14ca76f7 100644 --- a/controllers/easynav_mppi_controller/CHANGELOG.rst +++ b/controllers/easynav_mppi_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_mppi_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 1a5e4a5e..a0c85da5 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -2,7 +2,7 @@ easynav_mppi_controller - 0.2.0 + 0.2.1 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/controllers/easynav_serest_controller/CHANGELOG.rst b/controllers/easynav_serest_controller/CHANGELOG.rst index 616eb605..797b0426 100644 --- a/controllers/easynav_serest_controller/CHANGELOG.rst +++ b/controllers/easynav_serest_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_serest_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 30d1414d..66016d86 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -2,7 +2,7 @@ easynav_serest_controller - 0.2.0 + 0.2.1 Easy Navigation: SeReST Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_simple_controller/CHANGELOG.rst b/controllers/easynav_simple_controller/CHANGELOG.rst index eff04f45..7d3c92ef 100644 --- a/controllers/easynav_simple_controller/CHANGELOG.rst +++ b/controllers/easynav_simple_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index e80cacea..e347b895 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -2,7 +2,7 @@ easynav_simple_controller - 0.2.0 + 0.2.1 Easy Navigation: Simple Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_vff_controller/CHANGELOG.rst b/controllers/easynav_vff_controller/CHANGELOG.rst index 8c6ba653..d6f15f0e 100644 --- a/controllers/easynav_vff_controller/CHANGELOG.rst +++ b/controllers/easynav_vff_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_vff_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index 5d3ad153..7235a766 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -2,7 +2,7 @@ easynav_vff_controller - 0.2.0 + 0.2.1 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/localizers/easynav_costmap_localizer/CHANGELOG.rst b/localizers/easynav_costmap_localizer/CHANGELOG.rst index 425bdb64..b5c759e8 100644 --- a/localizers/easynav_costmap_localizer/CHANGELOG.rst +++ b/localizers/easynav_costmap_localizer/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_costmap_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index f7aebdf4..be30d15d 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_costmap_localizer - 0.2.0 + 0.2.1 Easy Navigation: Costmap Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 783ecf53..a245a547 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -2,7 +2,7 @@ easynav_fusion_localizer - 0.2.0 + 0.2.1 Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel diff --git a/localizers/easynav_gps_localizer/CHANGELOG.rst b/localizers/easynav_gps_localizer/CHANGELOG.rst index d38c68ce..1320050f 100644 --- a/localizers/easynav_gps_localizer/CHANGELOG.rst +++ b/localizers/easynav_gps_localizer/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_gps_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index b70cf111..b923c0ab 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -2,7 +2,7 @@ easynav_gps_localizer - 0.2.0 + 0.2.1 Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 3f494984..61524dca 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_navmap_localizer - 0.2.0 + 0.2.1 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_simple_localizer/CHANGELOG.rst b/localizers/easynav_simple_localizer/CHANGELOG.rst index a41bb621..cf2a976b 100644 --- a/localizers/easynav_simple_localizer/CHANGELOG.rst +++ b/localizers/easynav_simple_localizer/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index f6ef81ba..e9be42bd 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -2,7 +2,7 @@ easynav_simple_localizer - 0.2.0 + 0.2.1 Easy Navigation: Simple Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst index a96993f2..af483566 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_bonxai_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 847ff1be..2de0faf9 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_bonxai_maps_manager - 0.2.0 + 0.2.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst index 744f4ca0..28309613 100644 --- a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_costmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 6dbf5bdf..6f413b68 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_costmap_maps_manager - 0.2.0 + 0.2.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst index cf9fa781..feedad33 100644 --- a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_navmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index b49608be..925372c5 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_navmap_maps_manager - 0.2.0 + 0.2.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst index f1ed5768..d52660a8 100644 --- a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_octomap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index a5f2c18a..0b008d8a 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_octomap_maps_manager - 0.2.0 + 0.2.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst index ddc80c96..58f9c205 100644 --- a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * Remove C++20/C++23 features and update to new MethodBase interface * TFInfo in RTTFBuffer diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index b3545fa0..751d110a 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_routes_maps_manager - 0.2.0 + 0.2.1 Easy Navigation: Routes MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst index ae4428f1..9fa91ba3 100644 --- a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 97c372dd..7c5cda77 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_simple_maps_manager - 0.2.0 + 0.2.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_costmap_planner/CHANGELOG.rst b/planners/easynav_costmap_planner/CHANGELOG.rst index 59184072..f1d5e9af 100644 --- a/planners/easynav_costmap_planner/CHANGELOG.rst +++ b/planners/easynav_costmap_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_costmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 622e215b..98c5205f 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_costmap_planner - 0.2.0 + 0.2.1 Easy Navigation: Costmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_navmap_planner/CHANGELOG.rst b/planners/easynav_navmap_planner/CHANGELOG.rst index c9603fa6..e2f86b4f 100644 --- a/planners/easynav_navmap_planner/CHANGELOG.rst +++ b/planners/easynav_navmap_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_navmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 0356042c..8c8c538f 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_navmap_planner - 0.2.0 + 0.2.1 Easy Navigation: navmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_simple_planner/CHANGELOG.rst b/planners/easynav_simple_planner/CHANGELOG.rst index 8980db4c..fe6ca827 100644 --- a/planners/easynav_simple_planner/CHANGELOG.rst +++ b/planners/easynav_simple_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.1 (2026-02-27) +------------------ * 0.2.0 * GPLv3 -> Apache 2.0 * Documentation was corrected diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index 87fe4200..efd878fd 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -2,7 +2,7 @@ easynav_simple_planner - 0.2.0 + 0.2.1 Easy Navigation: Simple planner package. Francisco Martín Rico Apache-2.0 From 78d05eff75cb4cbc7620c01053de1a2c07a42d8e Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 27 Feb 2026 17:50:36 +0100 Subject: [PATCH 164/230] update changelogs Signed-off-by: Juan S. Cely G. --- common/easynav_costmap_common/package.xml | 2 +- common/easynav_simple_common/package.xml | 2 +- .../easynav_mpc_controller/CHANGELOG.rst | 26 +++++++++++++++++++ .../easynav_mpc_controller/package.xml | 2 +- .../easynav_mppi_controller/package.xml | 2 +- .../easynav_serest_controller/package.xml | 2 +- .../easynav_simple_controller/package.xml | 2 +- .../easynav_vff_controller/package.xml | 2 +- .../easynav_costmap_localizer/package.xml | 2 +- .../easynav_fusion_localizer/package.xml | 2 +- localizers/easynav_gps_localizer/package.xml | 2 +- .../easynav_navmap_localizer/package.xml | 2 +- .../easynav_simple_localizer/package.xml | 2 +- .../easynav_bonxai_maps_manager/package.xml | 2 +- .../easynav_costmap_maps_manager/package.xml | 2 +- .../easynav_navmap_maps_manager/package.xml | 2 +- .../easynav_octomap_maps_manager/package.xml | 2 +- .../easynav_routes_maps_manager/package.xml | 2 +- .../easynav_simple_maps_manager/package.xml | 2 +- planners/easynav_costmap_planner/package.xml | 2 +- planners/easynav_navmap_planner/package.xml | 2 +- planners/easynav_simple_planner/package.xml | 2 +- 22 files changed, 47 insertions(+), 21 deletions(-) create mode 100644 controllers/easynav_mpc_controller/CHANGELOG.rst diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 7f3c271f..768e4031 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -2,7 +2,7 @@ easynav_costmap_common - 0.0.2 + 0.3.0 Easy Navigation: Simple Common package. Francisco Martín Rico diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index 38ac4fb7..9011389a 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -2,7 +2,7 @@ easynav_simple_common - 0.0.2 + 0.3.0 Easy Navigation: Simple Common package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst new file mode 100644 index 00000000..fcda7e44 --- /dev/null +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -0,0 +1,26 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_mpc_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* GPLv3 -> Apache 2.0 +* Remove C++20/C++23 features and update to new MethodBase interface +* TFInfo in RTTFBuffer +* Refactor to use TFInfo +* Trim path for MPC +* Stop controllers at IDLE +* Obstacles are detected +* Methods were added to access to private attributes +* Optimizer header file was added +* Dependencies were fixed +* MPC with differential model is working +* MPC Controller minimizes path following +* Visualization Marker was added +* CI updated +* MPC controller is working +* Optimizer was changed +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, José Miguel Guerrero, Juan S. Cely G., Miguel + +0.0.2 (2025-10-12) +------------------ diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 6658226d..a10af3bb 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -2,7 +2,7 @@ easynav_mpc_controller - 0.0.1 + 0.3.0 Easy Navigation: MPC Controller package. Juan S. Cely G. Apache-2.0 diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index c3382a3c..54d37da6 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -2,7 +2,7 @@ easynav_mppi_controller - 0.0.2 + 0.3.0 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index a57476ed..cd19c4cf 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -2,7 +2,7 @@ easynav_serest_controller - 0.0.2 + 0.3.0 Easy Navigation: SeReST Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 99181b8b..922f1b0b 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -2,7 +2,7 @@ easynav_simple_controller - 0.0.2 + 0.3.0 Easy Navigation: Simple Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index 07b23911..30939d5a 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -2,7 +2,7 @@ easynav_vff_controller - 0.0.2 + 0.3.0 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index a8d85e0b..0cb4ab21 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_costmap_localizer - 0.0.2 + 0.3.0 Easy Navigation: Costmap Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 6d2175c6..70a7f2e2 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -2,7 +2,7 @@ easynav_fusion_localizer - 0.0.2 + 0.3.0 Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index 13d3d8d7..3fe29dfb 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -2,7 +2,7 @@ easynav_gps_localizer - 0.0.2 + 0.3.0 Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 96837eff..1626a1cb 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_navmap_localizer - 0.0.2 + 0.3.0 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index 16d61844..a4238d06 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -2,7 +2,7 @@ easynav_simple_localizer - 0.0.2 + 0.3.0 Easy Navigation: Simple Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 1ac676b4..10b4c04c 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_bonxai_maps_manager - 0.0.2 + 0.3.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 6d0cd57a..d6364947 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_costmap_maps_manager - 0.0.2 + 0.3.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index 6b29d112..c397af03 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_navmap_maps_manager - 0.0.2 + 0.3.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 158a6971..da49c8f9 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_octomap_maps_manager - 0.0.2 + 0.3.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index f824ed9b..4054848c 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_routes_maps_manager - 0.0.2 + 0.3.0 Easy Navigation: Routes MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index ed304e0c..5236f2eb 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_simple_maps_manager - 0.0.2 + 0.3.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 8d4eaa2c..07e1e653 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_costmap_planner - 0.0.2 + 0.3.0 Easy Navigation: Costmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 78db89d5..12979d75 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_navmap_planner - 0.0.2 + 0.3.0 Easy Navigation: navmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index f40d8b64..569f1183 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -2,7 +2,7 @@ easynav_simple_planner - 0.0.2 + 0.3.0 Easy Navigation: Simple planner package. Francisco Martín Rico Apache-2.0 From 06b6e1ccc98b829827770b1fe5e99eeaf377e1e7 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 27 Feb 2026 17:53:00 +0100 Subject: [PATCH 165/230] 0.3.1 --- common/easynav_costmap_common/package.xml | 2 +- common/easynav_simple_common/package.xml | 2 +- controllers/easynav_mpc_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_mpc_controller/package.xml | 2 +- controllers/easynav_mppi_controller/package.xml | 2 +- controllers/easynav_serest_controller/package.xml | 2 +- controllers/easynav_simple_controller/package.xml | 2 +- controllers/easynav_vff_controller/package.xml | 2 +- localizers/easynav_costmap_localizer/package.xml | 2 +- localizers/easynav_fusion_localizer/package.xml | 2 +- localizers/easynav_gps_localizer/package.xml | 2 +- localizers/easynav_navmap_localizer/package.xml | 2 +- localizers/easynav_simple_localizer/package.xml | 2 +- maps_managers/easynav_bonxai_maps_manager/package.xml | 2 +- maps_managers/easynav_costmap_maps_manager/package.xml | 2 +- maps_managers/easynav_navmap_maps_manager/package.xml | 2 +- maps_managers/easynav_octomap_maps_manager/package.xml | 2 +- maps_managers/easynav_routes_maps_manager/package.xml | 2 +- maps_managers/easynav_simple_maps_manager/package.xml | 2 +- planners/easynav_costmap_planner/package.xml | 2 +- planners/easynav_navmap_planner/package.xml | 2 +- planners/easynav_simple_planner/package.xml | 2 +- 22 files changed, 23 insertions(+), 23 deletions(-) diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 768e4031..9b6d8c74 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -2,7 +2,7 @@ easynav_costmap_common - 0.3.0 + 0.3.1 Easy Navigation: Simple Common package. Francisco Martín Rico diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index 9011389a..c02be4df 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -2,7 +2,7 @@ easynav_simple_common - 0.3.0 + 0.3.1 Easy Navigation: Simple Common package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst index fcda7e44..a8a04271 100644 --- a/controllers/easynav_mpc_controller/CHANGELOG.rst +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_mpc_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.1 (2026-02-27) +------------------ * GPLv3 -> Apache 2.0 * Remove C++20/C++23 features and update to new MethodBase interface * TFInfo in RTTFBuffer diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index a10af3bb..038ff4ed 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -2,7 +2,7 @@ easynav_mpc_controller - 0.3.0 + 0.3.1 Easy Navigation: MPC Controller package. Juan S. Cely G. Apache-2.0 diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 54d37da6..ff401d2c 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -2,7 +2,7 @@ easynav_mppi_controller - 0.3.0 + 0.3.1 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index cd19c4cf..83cfb24c 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -2,7 +2,7 @@ easynav_serest_controller - 0.3.0 + 0.3.1 Easy Navigation: SeReST Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 922f1b0b..7bf9b269 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -2,7 +2,7 @@ easynav_simple_controller - 0.3.0 + 0.3.1 Easy Navigation: Simple Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index 30939d5a..2de0dcf4 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -2,7 +2,7 @@ easynav_vff_controller - 0.3.0 + 0.3.1 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index 0cb4ab21..bc8b00d0 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_costmap_localizer - 0.3.0 + 0.3.1 Easy Navigation: Costmap Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 70a7f2e2..8c9f678e 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -2,7 +2,7 @@ easynav_fusion_localizer - 0.3.0 + 0.3.1 Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index 3fe29dfb..f7f6eff2 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -2,7 +2,7 @@ easynav_gps_localizer - 0.3.0 + 0.3.1 Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 1626a1cb..72286a33 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_navmap_localizer - 0.3.0 + 0.3.1 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index a4238d06..08526afb 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -2,7 +2,7 @@ easynav_simple_localizer - 0.3.0 + 0.3.1 Easy Navigation: Simple Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 10b4c04c..7ca25f29 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_bonxai_maps_manager - 0.3.0 + 0.3.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index d6364947..1cdf1446 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_costmap_maps_manager - 0.3.0 + 0.3.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index c397af03..c73f6f4e 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_navmap_maps_manager - 0.3.0 + 0.3.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index da49c8f9..e7fe9441 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_octomap_maps_manager - 0.3.0 + 0.3.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index 4054848c..0b9c9a6e 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_routes_maps_manager - 0.3.0 + 0.3.1 Easy Navigation: Routes MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 5236f2eb..0efe4f14 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_simple_maps_manager - 0.3.0 + 0.3.1 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 07e1e653..8471516d 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_costmap_planner - 0.3.0 + 0.3.1 Easy Navigation: Costmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 12979d75..7c297e25 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_navmap_planner - 0.3.0 + 0.3.1 Easy Navigation: navmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index 569f1183..fc12820b 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -2,7 +2,7 @@ easynav_simple_planner - 0.3.0 + 0.3.1 Easy Navigation: Simple planner package. Francisco Martín Rico Apache-2.0 From 100795395476d4bcd93c617c31b470d20cb7edf8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 2 Mar 2026 20:53:25 +0100 Subject: [PATCH 166/230] Fix segfault in some cases and reduce extrapolation to the future MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_serest_controller/SerestController.cpp | 14 ++++++++++---- .../easynav_simple_controller/SimpleController.cpp | 5 ++++- .../src/easynav_costmap_planner/CostmapPlanner.cpp | 4 ++-- .../src/easynav_simple_planner/SimplePlanner.cpp | 3 ++- 4 files changed, 18 insertions(+), 8 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 cfed5193..659dd4e6 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -309,8 +309,14 @@ SerestController::closest_obstacle_distance( .downsample(0.3); const auto & fused = view.as_points(); - if (last_input_ts_ < view.get_latest_stamp()) { - last_input_ts_ = view.get_latest_stamp(); + { + const auto view_latest = view.get_latest_stamp(); + const rclcpp::Time view_latest_same_clock( + view_latest.nanoseconds(), + last_input_ts_.get_clock_type()); + if (last_input_ts_ < view_latest_same_clock) { + last_input_ts_ = view_latest_same_clock; + } } double min_dist = std::numeric_limits::infinity(); @@ -378,10 +384,10 @@ SerestController::fetch_required_inputs( 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; + last_input_ts_ = rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()); } if (rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) { - last_input_ts_ = odom.header.stamp; + last_input_ts_ = rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()); } if (path.poses.empty()) { 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 aed490e7..6e80af64 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -111,7 +111,10 @@ SimpleController::update_rt(NavState & nav_state) const auto & pose = nav_state.get("robot_pose").pose.pose; const auto & goal_pose = path.poses.back().pose; - rclcpp::Time latest_stamp = nav_state.get("robot_pose").header.stamp; + const auto clock_type = get_node()->get_clock()->get_clock_type(); + rclcpp::Time latest_stamp( + nav_state.get("robot_pose").header.stamp, + clock_type); if (rclcpp::Time(path.poses.back().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { 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 4062a6d8..f6bfdf20 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -157,12 +157,12 @@ void CostmapPlanner::update(NavState & nav_state) 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; + latest_stamp = rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()); } if (rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { - latest_stamp = goals.goals.front().header.stamp; + latest_stamp = rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()); } current_path_.header.stamp = latest_stamp; 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 a619ecc8..b869c68c 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -130,7 +130,8 @@ SimplePlanner::update(NavState & nav_state) const auto & goal = goals.goals.front().pose; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - rclcpp::Time latest_stamp = robot_pose.header.stamp; + const auto clock_type = get_node()->get_clock()->get_clock_type(); + rclcpp::Time latest_stamp(robot_pose.header.stamp, clock_type); if (rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { From 13a55fe73d4c9c9408fe002adf97358c4315258a Mon Sep 17 00:00:00 2001 From: migueldm Date: Thu, 5 Mar 2026 12:03:13 +0100 Subject: [PATCH 167/230] params updated --- .../config/example_params_summit_dual.yaml | 42 ++++++++++++++++--- 1 file changed, 36 insertions(+), 6 deletions(-) diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml index 482386e8..40b6d005 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -1,7 +1,7 @@ controller_node: ros__parameters: use_sim_time: true - controller_types: [mppi] + controller_types: [serest] dummy: rt_freq: 30.0 plugin: easynav_controller/DummyController @@ -20,6 +20,30 @@ controller_node: max_linear_acceleration: 0.5 max_angular_acceleration: 0.5 safety_radius: 0.3 + serest: + rt_freq: 30.0 + plugin: easynav_serest_controller/SerestController + allow_reverse: true + max_linear_speed: 0.8 + max_angular_speed: 1.2 + v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado + k_s_share_max: 0.5 # succión lateral no cancela >50% del avance + k_theta: 2.5 # sube un poco + k_y: 1.5 + goal_pos_tol: 0.1 # 7 cm + goal_yaw_tol_deg: 6.0 # 6 grados + slow_radius: 0.60 + slow_min_speed: 0.03 + final_align_k: 2.0 + final_align_wmax: 0.6 + corner_guard_enable: true + corner_gain_ey: 1.8 + corner_gain_eth: 0.7 + corner_gain_kappa: 0.4 + corner_min_alpha: 0.35 + corner_boost_omega: 1.0 + a_lat_soft: 0.9 + apex_ey_des: 0.05 localizer_node: ros__parameters: @@ -48,6 +72,8 @@ localizer_node: base_link_frame: base_link world_frame: odom + dynamic_process_noise_covariance: true + #--- INPUT 2: Wheel Odometry --- odom0: /robotnik_base_control/odom odom0_config: @@ -213,23 +239,27 @@ maps_manager_node: planner_node: ros__parameters: use_sim_time: true - planner_types: [simple] + planner_types: [costmap] dummy: freq: 1.0 plugin: easynav_planner/DummyPlanner cycle_time_nort: 0.2 cycle_time_rt: 0.0001 - simple: + costmap: freq: 10.0 plugin: easynav_costmap_planner/CostmapPlanner cost_factor: 10.0 continuous_replan: true + simple: + freq: 0.5 + plugin: easynav_simple_planner/SimplePlanner + robot_radius: 0.3 sensors_node: ros__parameters: use_sim_time: true forget_time: 0.5 - sensors: [imu, gps, laser1] + sensors: [imu, gps] perception_default_frame: odom imu: topic: /imu/data @@ -246,5 +276,5 @@ sensors_node: system_node: ros__parameters: use_sim_time: true - position_tolerance: 0.1 - angle_tolerance: 0.05 + position_tolerance: 0.75 + angle_tolerance: 0.25 From 74770b506cd07a69354a1e176d0633872f6a4db8 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Tue, 10 Mar 2026 17:56:20 +0100 Subject: [PATCH 168/230] Run scheduled CI only in rolling. Add manual CI trigger. --- .github/workflows/humble.yaml | 1 + .github/workflows/humble_cron.yaml | 45 ----------------------------- .github/workflows/jazzy.yaml | 3 +- .github/workflows/jazzy_cron.yaml | 43 --------------------------- .github/workflows/kilted.yaml | 1 + .github/workflows/kilted_cron.yaml | 43 --------------------------- .github/workflows/rolling.yaml | 3 ++ .github/workflows/rolling_cron.yaml | 45 ----------------------------- 8 files changed, 6 insertions(+), 178 deletions(-) delete mode 100644 .github/workflows/humble_cron.yaml delete mode 100644 .github/workflows/jazzy_cron.yaml delete mode 100644 .github/workflows/kilted_cron.yaml delete mode 100644 .github/workflows/rolling_cron.yaml diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 51cdc493..246b9907 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -7,6 +7,7 @@ on: push: branches: - humble + workflow_dispatch: jobs: build-and-test: runs-on: ubuntu-22.04 diff --git a/.github/workflows/humble_cron.yaml b/.github/workflows/humble_cron.yaml deleted file mode 100644 index 82bb57ad..00000000 --- a/.github/workflows/humble_cron.yaml +++ /dev/null @@ -1,45 +0,0 @@ -name: humble - -on: - schedule: - - cron: '0 0 * * 6' -jobs: - build-and-test: - runs-on: ubuntu-22.04 - container: - image: ubuntu:jammy - steps: - - name: Repo checkout - uses: actions/checkout@v4 - with: - ref: humble - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 - with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller - target-ros2-distro: humble - ref: humble - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: false - colcon-defaults: | - { - "build": { - "packages-up-to": true, - "mixin": ["coverage-gcc"] - }, - "test": { - "parallel-workers" : 1 - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - - name: Codecov - uses: codecov/codecov-action@v5.4.0 - with: - files: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - # yml: ./codecov.yml - fail_ci_if_error: false diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 365cdccf..f12799f9 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -7,8 +7,7 @@ on: push: branches: - jazzy - schedule: - - cron: '0 0 * * 6' + workflow_dispatch: jobs: build-and-test: runs-on: ubuntu-24.04 diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml deleted file mode 100644 index c7049a98..00000000 --- a/.github/workflows/jazzy_cron.yaml +++ /dev/null @@ -1,43 +0,0 @@ -name: jazzy - -on: - schedule: - - cron: '0 0 * * 6' -jobs: - build-and-test: - runs-on: ubuntu-24.04 - container: - image: ubuntu:noble - steps: - - name: Repo checkout - uses: actions/checkout@v4 - with: - ref: jazzy - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 - with: - package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller - target-ros2-distro: jazzy - skip-tests: false - colcon-defaults: | - { - "build": { - "packages-up-to": true, - "mixin": ["coverage-gcc"] - }, - "test": { - "parallel-workers" : 1 - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - - name: Codecov - uses: codecov/codecov-action@v5.4.0 - with: - files: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - # yml: ./codecov.yml - fail_ci_if_error: false diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 0a4f68ed..ee4356be 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -7,6 +7,7 @@ on: push: branches: - kilted + workflow_dispatch: jobs: build-and-test: runs-on: ubuntu-24.04 diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml deleted file mode 100644 index c4b6b82b..00000000 --- a/.github/workflows/kilted_cron.yaml +++ /dev/null @@ -1,43 +0,0 @@ -name: kilted - -on: - schedule: - - cron: '0 0 * * 6' -jobs: - build-and-test: - runs-on: ubuntu-24.04 - container: - image: ubuntu:noble - steps: - - name: Repo checkout - uses: actions/checkout@v4 - with: - ref: kilted - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 - with: - package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller - target-ros2-distro: kilted - skip-tests: false - colcon-defaults: | - { - "build": { - "packages-up-to": true, - "mixin": ["coverage-gcc"] - }, - "test": { - "parallel-workers" : 1 - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - - name: Codecov - uses: codecov/codecov-action@v5.4.0 - with: - files: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - # yml: ./codecov.yml - fail_ci_if_error: false diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 7efd701a..d70f2b65 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -7,6 +7,9 @@ on: push: branches: - rolling + schedule: + - cron: '0 0 * * 6' + workflow_dispatch: jobs: build-and-test: runs-on: ubuntu-24.04 diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml deleted file mode 100644 index ef59286d..00000000 --- a/.github/workflows/rolling_cron.yaml +++ /dev/null @@ -1,45 +0,0 @@ -name: rolling - -on: - schedule: - - cron: '0 0 * * 6' -jobs: - build-and-test: - runs-on: ubuntu-24.04 - container: - image: ubuntu:noble - steps: - - name: Repo checkout - uses: actions/checkout@v6-beta - with: - ref: rolling - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 - with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller - target-ros2-distro: rolling - ref: rolling - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: false - colcon-defaults: | - { - "build": { - "packages-up-to": true, - "mixin": ["coverage-gcc"] - }, - "test": { - "parallel-workers" : 1 - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - - name: Codecov - uses: codecov/codecov-action@v5.4.0 - with: - files: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - # yml: ./codecov.yml - fail_ci_if_error: false From 6a0f0f2232c2425986e105956a5b30b5d17d7071 Mon Sep 17 00:00:00 2001 From: migueldm Date: Wed, 11 Mar 2026 15:02:32 +0100 Subject: [PATCH 169/230] Added default path for route if not specified --- .../RoutesMapsManager.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) 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 4af7b510..3c0645f3 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 @@ -74,11 +74,12 @@ void RoutesMapsManager::on_initialize() map_path_.clear(); if (package_name.empty() && map_path_file.empty()) { - // Accepted: we will use a default in-memory route in load_routes_from_yaml(). + // No path configured: default to /tmp so that save_routes still works. + map_path_ = "/tmp/routes.yaml"; RCLCPP_INFO( node->get_logger(), - "[%s] No package or map_path_file specified, using default in-memory route", - plugin_name.c_str()); + "[%s] No package or map_path_file specified, routes will be saved to %s", + plugin_name.c_str(), map_path_.c_str()); } else if (!map_path_file.empty() && map_path_file[0] == '/') { // Absolute path: ignore package_name. map_path_ = map_path_file; @@ -161,11 +162,16 @@ void RoutesMapsManager::on_initialize() out << YAML::EndMap; std::ofstream file(map_path_); + if (!file.is_open()) { + response->success = false; + response->message = "Could not open file for writing: " + map_path_; + return; + } file << out.c_str(); file.close(); response->success = true; - response->message = "Routes saved"; + response->message = "Routes saved to " + map_path_; } catch (const std::exception & e) { response->success = false; response->message = e.what(); From b75fcc47d2da336a92338eeb803c47b4747d6d96 Mon Sep 17 00:00:00 2001 From: migueldm Date: Fri, 13 Mar 2026 11:35:17 +0100 Subject: [PATCH 170/230] Only starts filter if params are found --- .../FusionLocalizer.hpp | 4 +- .../FusionLocalizer.cpp | 196 +++++++++++------- 2 files changed, 125 insertions(+), 75 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 19f829b6..1f49c3f8 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -58,6 +58,9 @@ class FusionLocalizer : public easynav::LocalizerMethodBase std::unique_ptr ukf_local_{nullptr}; std::unique_ptr ukf_global_{nullptr}; + bool has_global_filter_{false}; + bool has_local_filter_{false}; + int n_gps_sensors_{0}; geometry_msgs::msg::PoseWithCovarianceStamped @@ -79,7 +82,6 @@ class FusionLocalizer : public easynav::LocalizerMethodBase std::vector last_gps_stamp_; rclcpp::Publisher::SharedPtr navsat_pub_{nullptr}; - rclcpp::Publisher::SharedPtr gps_debug_pub_{nullptr}; std::string navsatfix_topic_; bool first_pose_received_{false}; 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 825c6a09..e9dbee35 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -31,47 +31,89 @@ void FusionLocalizer::on_initialize() RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'", plugin_name.c_str()); - ukf_global_ = std::make_unique( - localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false - ); - ukf_local_ = std::make_unique( - localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true - ); - ukf_global_->initialize(); - ukf_local_->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_); - - localizer_node->declare_parameter( - plugin_name + ".navsatfix_topic", std::string("global/navsatfix")); - localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); - navsat_pub_ = localizer_node->create_publisher( - navsatfix_topic_, rclcpp::QoS(10)); - gps_debug_pub_ = - localizer_node->create_publisher( - "gps_pose", rclcpp::QoS(10)); + // Detect which filters have parameters configured by checking + // parameter overrides (loaded from YAML before declaration) + const std::string global_prefix = plugin_name + ".global_filter."; + const std::string local_prefix = plugin_name + ".local_filter."; + + const auto & overrides = + localizer_node->get_node_parameters_interface()->get_parameter_overrides(); + + for (const auto & [key, _] : overrides) { + if (!has_global_filter_ && key.rfind(global_prefix, 0) == 0) { + has_global_filter_ = true; + } + if (!has_local_filter_ && key.rfind(local_prefix, 0) == 0) { + has_local_filter_ = true; + } + if (has_global_filter_ && has_local_filter_) { + break; + } + } + + if (!has_global_filter_ && !has_local_filter_) { + RCLCPP_FATAL( + localizer_node->get_logger(), + "No parameters found for either '%s' or '%s'. " + "At least one filter must be configured.", + global_prefix.c_str(), local_prefix.c_str()); + throw std::runtime_error( + "FusionLocalizer: no global_filter or local_filter parameters detected. " + "At least one filter must be configured."); + } + + if (has_global_filter_) { + ukf_global_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false + ); + ukf_global_->initialize(); + } else { + RCLCPP_WARN(localizer_node->get_logger(), "No global_filter parameters found. Global filter will NOT be created."); + } + + if (has_local_filter_) { + ukf_local_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true + ); + ukf_local_->initialize(); + } else { + RCLCPP_WARN(localizer_node->get_logger(), "No local_filter parameters found. Local filter will NOT be created."); + } + + // GPS-related setup only needed when global filter is active + if (has_global_filter_) { + 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_); + + localizer_node->declare_parameter( + plugin_name + ".navsatfix_topic", std::string("gps/filtered")); + localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); + navsat_pub_ = localizer_node->create_publisher( + navsatfix_topic_, rclcpp::QoS(10)); + } + } catch (const std::exception & e) { RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", e.what()); - // Raise error throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what()); } + if (has_global_filter_) { + GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_, + UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); + UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S"); + UTM_origin_z_ = altitude_origin_; - GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_, - UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); - UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S"); - UTM_origin_z_ = altitude_origin_; - - n_gps_sensors_ = static_cast(ukf_global_->getGpsCallbackDataArr().size()); - last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + n_gps_sensors_ = static_cast(ukf_global_->getGpsCallbackDataArr().size()); + last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + } RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); } @@ -80,55 +122,61 @@ void FusionLocalizer::update_rt(NavState & nav_state) { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - if (n_gps_sensors_ && nav_state.has("gnss")) { - auto gps_data = nav_state.get(std::string("gnss")); - const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); - for (int i = 0; i < n_gps_sensors_; ++i) { - if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { - continue; - } - rclcpp::Time gps_time(gps_data[i]->data.header.stamp); - if (gps_time > last_gps_stamp_[i]) { - EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); - last_gps_stamp_[i] = gps_time; - auto pose = - std::make_shared(navsatfix_to_pose( - gps_data[i]->data)); - if (!first_pose_received_) { - RCLCPP_INFO(get_node()->get_logger(), - "First valid GPS fix received. Initializing filter state."); - if(nav_state.has("imu")) { - auto imu_data = nav_state.get(std::string("imu")); - if (!imu_data.empty()) { - pose->pose.pose.orientation = imu_data[0]->data.orientation; + if (has_global_filter_) { + if (n_gps_sensors_ && nav_state.has("gnss")) { + auto gps_data = nav_state.get(std::string("gnss")); + const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); + for (int i = 0; i < n_gps_sensors_; ++i) { + if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + continue; + } + rclcpp::Time gps_time(gps_data[i]->data.header.stamp); + if (gps_time > last_gps_stamp_[i]) { + EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); + last_gps_stamp_[i] = gps_time; + auto pose = + std::make_shared(navsatfix_to_pose( + gps_data[i]->data)); + if (!first_pose_received_) { + RCLCPP_INFO(get_node()->get_logger(), + "First valid GPS fix received. Initializing filter state."); + if(nav_state.has("imu")) { + auto imu_data = nav_state.get(std::string("imu")); + if (!imu_data.empty()) { + pose->pose.pose.orientation = imu_data[0]->data.orientation; + } } + ukf_global_->setPoseCallback(pose); + first_pose_received_ = true; + continue; } - ukf_global_->setPoseCallback(pose); - first_pose_received_ = true; - continue; + ukf_global_->poseCallback( + pose, + gps_cb_arr[i], + tf_info.map_frame, + gps_data[i]->data.header.frame_id, + false + ); } - gps_debug_pub_->publish(*pose); - ukf_global_->poseCallback( - pose, - gps_cb_arr[i], - tf_info.map_frame, - gps_data[i]->data.header.frame_id, - false - ); } } - } - ukf_global_->periodicUpdate(); - ukf_local_->periodicUpdate(); + ukf_global_->periodicUpdate(); - nav_msgs::msg::Odometry global_odom, local_odom; - if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { - nav_state.set("robot_pose", global_odom); - navsat_pub_->publish(odom_to_navsatfix(global_odom)); + nav_msgs::msg::Odometry global_odom; + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + nav_state.set("robot_pose", global_odom); + navsat_pub_->publish(odom_to_navsatfix(global_odom)); + } } - if (ukf_local_->getFilteredOdometryMessage(&local_odom)) { - nav_state.set("robot_pose_local", local_odom); + + if (has_local_filter_) { + ukf_local_->periodicUpdate(); + + nav_msgs::msg::Odometry local_odom; + if (ukf_local_->getFilteredOdometryMessage(&local_odom)) { + nav_state.set("robot_pose_local", local_odom); + } } } From f2bfb9bb02d60506018b452dec848a9736c93d5b Mon Sep 17 00:00:00 2001 From: migueldm Date: Fri, 13 Mar 2026 11:36:57 +0100 Subject: [PATCH 171/230] Uncrustify changes --- .../src/easynav_fusion_localizer/FusionLocalizer.cpp | 6 ++++-- 1 file changed, 4 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 e9dbee35..7982a9a7 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -68,7 +68,8 @@ void FusionLocalizer::on_initialize() ); ukf_global_->initialize(); } else { - RCLCPP_WARN(localizer_node->get_logger(), "No global_filter parameters found. Global filter will NOT be created."); + RCLCPP_WARN(localizer_node->get_logger(), + "No global_filter parameters found. Global filter will NOT be created."); } if (has_local_filter_) { @@ -77,7 +78,8 @@ void FusionLocalizer::on_initialize() ); ukf_local_->initialize(); } else { - RCLCPP_WARN(localizer_node->get_logger(), "No local_filter parameters found. Local filter will NOT be created."); + RCLCPP_WARN(localizer_node->get_logger(), + "No local_filter parameters found. Local filter will NOT be created."); } // GPS-related setup only needed when global filter is active From 1a70c12eb57485de120e267b4e1b9a90e77f7ad4 Mon Sep 17 00:00:00 2001 From: migueldm Date: Fri, 13 Mar 2026 11:37:51 +0100 Subject: [PATCH 172/230] Added config file for gazebo smulation demo --- .../example_params_summit_dual_gazebo.yaml | 280 ++++++++++++++++++ 1 file changed, 280 insertions(+) create mode 100644 localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml new file mode 100644 index 00000000..b7afad57 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml @@ -0,0 +1,280 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [serest] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + mppi: + rt_freq: 30.0 + plugin: easynav_mppi_controller/MPPIController + num_samples: 200 + horizon_steps: 5 + dt: 0.15 + lambda: 1.0 + fov: 1.57 + max_linear_velocity: 1.5 + max_angular_velocity: 1.0 + max_linear_acceleration: 0.5 + max_angular_acceleration: 0.5 + safety_radius: 0.3 + serest: + rt_freq: 30.0 + plugin: easynav_serest_controller/SerestController + allow_reverse: true + max_linear_speed: 0.8 + max_angular_speed: 1.2 + v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado + k_s_share_max: 0.5 # succión lateral no cancela >50% del avance + k_theta: 2.5 # sube un poco + k_y: 1.5 + goal_pos_tol: 0.1 # 7 cm + goal_yaw_tol_deg: 6.0 # 6 grados + slow_radius: 0.60 + slow_min_speed: 0.03 + final_align_k: 2.0 + final_align_wmax: 0.6 + corner_guard_enable: true + corner_gain_ey: 1.8 + corner_gain_eth: 0.7 + corner_gain_kappa: 0.4 + corner_min_alpha: 0.35 + corner_boost_omega: 1.0 + a_lat_soft: 0.9 + apex_ey_des: 0.05 + +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.284796759605115 + longitude_origin: -3.8214099132625017 + altitude_origin: 669.9730248888955 + local_filter: + two_d_mode: true + publish_tf: false + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + dynamic_process_noise_covariance: true + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + # imu0: /imu/data + # imu0_config: + # [false, false, false, + # false, false, false, + # false, false, false, + # false, false, true, + # true, true, true] + # 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] + global_filter: + two_d_mode: true + publish_tf: true + debug: true + debug_out_file: /tmp/ukf_global_debug.txt + transform_timeout: 0.1 + smooth_lagged_data: true + # Defines the time window in seconds to keep past states in memory. + # history_length: 5.0 + dynamic_process_noise_covariance: 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, + false, false, false, + false, false, false, + false, false, 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, true, 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] + + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + filters: [routes_costmap] + package: easynav_fusion_localizer + map_path_file: routes/simple_square.yaml + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 50 + route_width: 0.5 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easynav_fusion_localizer + map_path_file: maps/empty.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + 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: [costmap] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + costmap: + freq: 10.0 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + simple: + freq: 0.5 + plugin: easynav_simple_planner/SimplePlanner + robot_radius: 0.3 + +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 + laser1: + topic: /front_laser/points + type: sensor_msgs/msg/PointCloud2 + group: points + + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.75 + angle_tolerance: 0.25 From 76f1f93c6a34643de8ed199ca38666145f1c22df Mon Sep 17 00:00:00 2001 From: migueldm Date: Fri, 13 Mar 2026 12:05:51 +0100 Subject: [PATCH 173/230] Port from rolling to jazzy --- .../easynav_fusion_localizer/CMakeLists.txt | 4 + .../config/example_params.yaml | 2 +- .../config/example_params_summit_dual.yaml | 280 ++++++++++++++++++ .../example_params_summit_dual_gazebo.yaml | 280 ++++++++++++++++++ .../FusionLocalizer.hpp | 25 +- .../easynav_fusion_localizer/ukf_wrapper.hpp | 15 +- .../easynav_fusion_localizer/maps/empty.png | Bin 0 -> 4849 bytes .../easynav_fusion_localizer/maps/empty.yaml | 7 + .../easynav_fusion_localizer/maps/square.png | Bin 0 -> 11002 bytes .../easynav_fusion_localizer/maps/square.yaml | 7 + .../easynav_fusion_localizer/package.xml | 2 +- .../FusionLocalizer.cpp | 252 +++++++++++----- .../easynav_fusion_localizer/ukf_wrapper.cpp | 62 ++-- 13 files changed, 833 insertions(+), 103 deletions(-) create mode 100644 localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml create mode 100644 localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml create mode 100644 localizers/easynav_fusion_localizer/maps/empty.png create mode 100644 localizers/easynav_fusion_localizer/maps/empty.yaml create mode 100644 localizers/easynav_fusion_localizer/maps/square.png create mode 100644 localizers/easynav_fusion_localizer/maps/square.yaml diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index 4db3bed8..94db9550 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -72,6 +72,10 @@ install(TARGETS RUNTIME DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY maps/ + DESTINATION share/${PROJECT_NAME}/maps +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) diff --git a/localizers/easynav_fusion_localizer/config/example_params.yaml b/localizers/easynav_fusion_localizer/config/example_params.yaml index 9c47f30b..905be4e5 100644 --- a/localizers/easynav_fusion_localizer/config/example_params.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params.yaml @@ -25,7 +25,7 @@ localizer_node: latitude_origin: 40.2834931 longitude_origin: -3.8207253999999997 altitude_origin: 746.184 - local_filter: + global_filter: two_d_mode: true publish_tf: true diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml new file mode 100644 index 00000000..40b6d005 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -0,0 +1,280 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [serest] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + mppi: + rt_freq: 30.0 + plugin: easynav_mppi_controller/MPPIController + num_samples: 200 + horizon_steps: 5 + dt: 0.15 + lambda: 1.0 + fov: 1.57 + max_linear_velocity: 1.5 + max_angular_velocity: 1.0 + max_linear_acceleration: 0.5 + max_angular_acceleration: 0.5 + safety_radius: 0.3 + serest: + rt_freq: 30.0 + plugin: easynav_serest_controller/SerestController + allow_reverse: true + max_linear_speed: 0.8 + max_angular_speed: 1.2 + v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado + k_s_share_max: 0.5 # succión lateral no cancela >50% del avance + k_theta: 2.5 # sube un poco + k_y: 1.5 + goal_pos_tol: 0.1 # 7 cm + goal_yaw_tol_deg: 6.0 # 6 grados + slow_radius: 0.60 + slow_min_speed: 0.03 + final_align_k: 2.0 + final_align_wmax: 0.6 + corner_guard_enable: true + corner_gain_ey: 1.8 + corner_gain_eth: 0.7 + corner_gain_kappa: 0.4 + corner_min_alpha: 0.35 + corner_boost_omega: 1.0 + a_lat_soft: 0.9 + apex_ey_des: 0.05 + +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: odom + + dynamic_process_noise_covariance: true + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + # imu0: /imu/data + # imu0_config: + # [false, false, false, + # false, false, false, + # false, false, false, + # false, false, true, + # true, true, true] + # 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] + global_filter: + two_d_mode: true + publish_tf: true + debug: true + debug_out_file: /tmp/ukf_global_debug.txt + transform_timeout: 0.1 + smooth_lagged_data: true + # Defines the time window in seconds to keep past states in memory. + # history_length: 5.0 + dynamic_process_noise_covariance: 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, + false, false, false, + false, false, false, + false, false, 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, true, 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] + + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + filters: [routes_costmap] + package: easynav_fusion_localizer + map_path_file: routes/simple_square.yaml + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 50 + route_width: 0.5 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easynav_fusion_localizer + map_path_file: maps/square.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + 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: [costmap] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + costmap: + freq: 10.0 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + simple: + freq: 0.5 + plugin: easynav_simple_planner/SimplePlanner + robot_radius: 0.3 + +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 + laser1: + topic: /front_laser/points + type: sensor_msgs/msg/PointCloud2 + group: points + + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.75 + angle_tolerance: 0.25 diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml new file mode 100644 index 00000000..b7afad57 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml @@ -0,0 +1,280 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [serest] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + mppi: + rt_freq: 30.0 + plugin: easynav_mppi_controller/MPPIController + num_samples: 200 + horizon_steps: 5 + dt: 0.15 + lambda: 1.0 + fov: 1.57 + max_linear_velocity: 1.5 + max_angular_velocity: 1.0 + max_linear_acceleration: 0.5 + max_angular_acceleration: 0.5 + safety_radius: 0.3 + serest: + rt_freq: 30.0 + plugin: easynav_serest_controller/SerestController + allow_reverse: true + max_linear_speed: 0.8 + max_angular_speed: 1.2 + v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado + k_s_share_max: 0.5 # succión lateral no cancela >50% del avance + k_theta: 2.5 # sube un poco + k_y: 1.5 + goal_pos_tol: 0.1 # 7 cm + goal_yaw_tol_deg: 6.0 # 6 grados + slow_radius: 0.60 + slow_min_speed: 0.03 + final_align_k: 2.0 + final_align_wmax: 0.6 + corner_guard_enable: true + corner_gain_ey: 1.8 + corner_gain_eth: 0.7 + corner_gain_kappa: 0.4 + corner_min_alpha: 0.35 + corner_boost_omega: 1.0 + a_lat_soft: 0.9 + apex_ey_des: 0.05 + +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.284796759605115 + longitude_origin: -3.8214099132625017 + altitude_origin: 669.9730248888955 + local_filter: + two_d_mode: true + publish_tf: false + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + dynamic_process_noise_covariance: true + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + # imu0: /imu/data + # imu0_config: + # [false, false, false, + # false, false, false, + # false, false, false, + # false, false, true, + # true, true, true] + # 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] + global_filter: + two_d_mode: true + publish_tf: true + debug: true + debug_out_file: /tmp/ukf_global_debug.txt + transform_timeout: 0.1 + smooth_lagged_data: true + # Defines the time window in seconds to keep past states in memory. + # history_length: 5.0 + dynamic_process_noise_covariance: 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, + false, false, false, + false, false, false, + false, false, 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, true, 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] + + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + filters: [routes_costmap] + package: easynav_fusion_localizer + map_path_file: routes/simple_square.yaml + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 50 + route_width: 0.5 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easynav_fusion_localizer + map_path_file: maps/empty.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + 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: [costmap] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + costmap: + freq: 10.0 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + simple: + freq: 0.5 + plugin: easynav_simple_planner/SimplePlanner + robot_radius: 0.3 + +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 + laser1: + topic: /front_laser/points + type: sensor_msgs/msg/PointCloud2 + group: points + + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.75 + angle_tolerance: 0.25 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 c29ce6e9..1f49c3f8 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -1,17 +1,21 @@ #pragma once #include +#include #include "easynav_core/LocalizerMethodBase.hpp" -#include "easynav_fusion_localizer/ukf_wrapper.hpp" // Tu wrapper refactorizado +#include "easynav_fusion_localizer/ukf_wrapper.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" namespace easynav { +constexpr double kNoOrientationCovariance = 1e5; + /** * @class FusionLocalizer * @brief Plugin de localización para EasyNav que integra el UKF de @@ -51,14 +55,20 @@ class FusionLocalizer : public easynav::LocalizerMethodBase void update(NavState & nav_state) override; private: - std::unique_ptr ukf_wrapper_; + std::unique_ptr ukf_local_{nullptr}; + std::unique_ptr ukf_global_{nullptr}; + + bool has_global_filter_{false}; + bool has_local_filter_{false}; - int n_imu_sensors_{0}; int n_gps_sensors_{0}; geometry_msgs::msg::PoseWithCovarianceStamped navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg); + sensor_msgs::msg::NavSatFix + odom_to_navsatfix(const nav_msgs::msg::Odometry & odom_msg); + double latitude_origin_{0.0}; double longitude_origin_{0.0}; double altitude_origin_{0.0}; @@ -66,8 +76,15 @@ class FusionLocalizer : public easynav::LocalizerMethodBase double UTM_origin_y_{0.0}; double UTM_origin_z_{0.0}; std::string UTM_zone_; + int UTM_zone_number_{0}; + bool UTM_zone_northp_{true}; + + std::vector last_gps_stamp_; + + rclcpp::Publisher::SharedPtr navsat_pub_{nullptr}; + std::string navsatfix_topic_; - std::vector last_gps_stamp_; + bool first_pose_received_{false}; }; 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 d9dbde9c..1c7f4d35 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 @@ -64,7 +64,6 @@ #include "tf2/LinearMath/Transform.hpp" #include #include -#include namespace robot_localization { @@ -108,7 +107,8 @@ class UkfWrapper explicit UkfWrapper( std::shared_ptr parent_node, const std::string & tf_prefix, - const std::string & plugin_name); + const std::string & plugin_name, + bool local_filter); //! @brief Destructor //! @@ -344,7 +344,7 @@ class UkfWrapper //! bool validateFilterOutput(nav_msgs::msg::Odometry * message); - std::vector getGpsCallbackDataArr() const + const std::vector & getGpsCallbackDataArr() const { return gps_callbackData_arr_; } @@ -820,13 +820,9 @@ class UkfWrapper //! rclcpp::Service::SharedPtr reset_srv_; - //! @brief Transform buffer for managing coordinate transforms + //! @brief Transform buffer for managing coordinate transforms (shared RTTFBuffer singleton) //! - std::unique_ptr tf_buffer_; - - //! @brief Transform listener for receiving transforms - //! - std::unique_ptr tf_listener_; + tf2_ros::Buffer * tf_buffer_{nullptr}; //! @brief broadcaster of worldTransform tfs //! @@ -870,6 +866,7 @@ class UkfWrapper std::string plugin_name_; std::string tf_prefix_; std::vector gps_callbackData_arr_; + bool local_filter_; }; diff --git a/localizers/easynav_fusion_localizer/maps/empty.png b/localizers/easynav_fusion_localizer/maps/empty.png new file mode 100644 index 0000000000000000000000000000000000000000..cd6eb69a9179a64f6596a543a2ed9dbc40bb35fc GIT binary patch literal 4849 zcmeAS@N?(olHy`uVBq!ia0y~yV15C@985rwLkFEVGcYi=WI8(scse^P6cpvBW#*(Z zFlbDyow(oou!GFe_+ZzGT}`rG7AY(I$^s|$ZRK)ZA&}J?ChMd3m(^Ell8Ctb!L1J- z^dDW-+`V}n-?}D+AIwMW9p;F{E;;gEwN=bePUic=yL*q{wRXq|o0;}ig~50GSsyLU zXP;Il-PdMuG3c>inHhO(j>^)9*8AQGK6oA5lm2u5?_!=+tlf-^6D(rok0%0~tpBVHvf?T);8r6av#@v3FP z`zCyjc)I6k2V|b=O&QPSLMP$9B zB?_pvgB2j~j0_V%DHTv+VzfkoH5HJP6RnftXo*6Fh89XX9xYL@rDGzBMG~v`(GmsP zfxum)4oFfQEm1~G6fBK2!f6pD9TV;_jFu=^8(N@nh6f(uzAM(|a>H!ah6Jv&ReOqb PL4ziqu6{1-oD!M`cqa44*@+DSSXO-M)r2{>TH=RKZ- zI)W&x!YB&rIEn`fjv_h^qRZ~W0BXEPxuT2;D!5;F5``Uo`y6%u+3+5rySl2rs$c!; ztFOb#i0MHC?Op952pSj?91saXHoxM(eeHmRzv~}&z#l_ebgViO)xZj+Tr81dusT(N z!I)Meh9GVInlERqwH?#<<+C9wTiZbH+OBU!@`IH(B9qe6HijJi(>xD}2W?xKU$x;^ z{ZC`=M*cIpU-0IyM`VT$ZZK^4yTK-5Y|_*7%WJKcJiPkzg}R(vy;D$i)$(&OWv)dq zE1;7%ud1``brZx|43tTO0zoX)PxYU$~7!Ad|LjV zF~!sN&fO6-{CVRqQ+Ig}e@>clI@Be*Bg?~ooXL1&KW_3J7Mm#jWq>`yEX9ER(^xNG~7)b7&q2;+;-$a z=DBsVXL@Jco~?+}Z$%lo+2b4ruU^WUQUC2w{u*sIXXkw0-Tp4?VlL0xc0{?!tL@?( z=bs*ozcl+h?kd}1hivihN#Zcszyxhb?UWB9-WhPJ;m3tbjMXTw)o5}_}INC|7xs!V6(oUcq6;~%oyaadCgB8 z+FB)TEjJid-5Jo7cI7vzS6602kfW~zkTI4&jVF}LD5yv-z$jXo0F&R)*fmLa}mJABvN4&PcAxMzO z6oeH3yO*X~B7Or_uh{S>L^?eK0p_p$_R==zPACH{KA#sL7bfH3g#`GJ@b!5jxlkhF z5tkeRmrLWaxMVidOF(8K7>z8TalFVZwur$6)IeAqMh{mZi7GXkD8z7Az&S+%{9ppM z2oo~dWHt+YFcATrEMU_ZWR6(CMQL;qSIG40;UZiq0jxx&J)^>1iGVAH0Oc^nVg{MU zpb5!L4qym}a;XmqcLC#;uOIV4JBd&=8i1-X;1r>= zcnF18V zm0*}?3LYvnrl3MoFcA?a@MU}?oQEbRU|{z!(bX)MyoCxhp@7N8ONW9|k(nYkMiu}O zS;Xc7sesEwX=rcnD!Ev#L6z9#1b`#J3ec0l3icoh<=I|O7baxOda(pmw;i*BJYK-1uhF*?}fm7DO=cOf$O~xcrRrO zyFMCR_HSN_u|#l|)qq#tddD*q@Vaa#2nz}zUYwy$|C5WrRX;^=oC<;{?)a~jHs3D= z2>Yl*_cCjFzg&jDTS3H;9}~eEiFzK4ANgl4b{Gyp&Z|QL zCP!=QTdvVoHP*QS!p3q~%<&V_e@^uO)3LPb*=1Yk<$>gQP5a?#HAB*GRC-tY zb!DT@x757j3aI>M{)*TtsC<-7dM!22)5Qv^%L;0I`vC7)UEK6Fv4>tLlDL8+^Se)? z`rLj{$AQ8^R#%2+ep%PE;->CLHpJ_W%ydbs3t+ZrOs3$pDAAYAK_I-y6WlLnt1#yC2fPss5hscn)_5RrNrA*zL!v{<3qms-pPLcmENf_)(frHv)0)LrI7YMymm-^g zxtE$k+!D#^cE-eDV3IC&n3O1MaJ9m{ub&lPJC%4(Q4BbXI9ox-Rujdodo15lR$6+l zrY34bLbzuqW(BqWIMVd?lC|ef*%Al5PMoisp3FD7+;XXBDU6sV6R`tDj~dR}<(+Xk z1xaU!0JdJff7poRaUVZFfRMT6b^dz`f9i{;t81)3>SYZb?C(jM!GHjBPPy&QTIKU> z!(}_jZ8cz1GwV02w;Wq_!uC-hP=JBMyLe&@?sS2KZv76n8Tmx$9XrsYb+W5Y*M2{t zqLn#d$(&($(24=IO-Jeu=T09t+!-&p+u6HH8c{Cb!aQJ}&UFhi5;h|!CMGBSKzpY& z0@B@BPZD_9lnX~x(jK0;esAg6eE;0^_*y}=9nL9-dtMv90PWFw*1Z+--cJNUP>tx{ zUdI5Bn$4qGecqgqp{VKLC$??aj1PCW1Be#v^dV zvNJ?^_0H#$COnmXS99?*JG_dO3f~=|s~&KL1?%26}DSsFpss|3F^7d+RKpdPf|lx=jr&Vw2IO?*Z-c zMmt$p2>3uW+EEz@1?XKUDsEctT`~^5z=K*&)L$et%N@KQeq)1cnrQ-3CVJ8^Y%^x8 zK3vkXGxF;W^K0%{+u&P{vgyTj6T$DLvIqilmL<8F7HlE*3%oNM+S^Sn(pZfQ08QFT z2|PF7PFH!PxS@0a(F#}bR`>?A0ue0uG@TH2koC<9fmQd(_aj0_kPt;LQI~P$M?HB9_?Cf2Q^P$)pyZ| zKKjKio#DIr_<)j+>b}hZQ{vUj3i{YG4;&(Wl1)0Bx)~&{wYk2;!BAd0e1tIyevJ`$ zuy(JV*fjrvP&c|)NeumKn8q!-aVFTdI+hYcqI9L^&yaSuXa1ZFer@tqX4R{%?83sG zvIf$UMcWgMLBM4-52AmL8`b4E0@9_2IUUNzUqc>0#y1Mfmo!Eq_ack=Tb4f}1Qauy zTaL4)jp2g easynav_fusion_localizer - 0.2.1 + 0.0.2 Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel 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 b212b73f..7982a9a7 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -6,6 +6,7 @@ #include "easynav_common/types/IMUPerception.hpp" #include "easynav_common/types/GNSSPerception.hpp" +#include "sensor_msgs/msg/nav_sat_status.hpp" #include @@ -23,8 +24,6 @@ void FusionLocalizer::on_initialize() auto localizer_node = std::dynamic_pointer_cast(node); - last_gps_stamp_.resize(10, 0.0); - const std::string & plugin_name = this->get_plugin_name(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -32,75 +31,157 @@ void FusionLocalizer::on_initialize() RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'", plugin_name.c_str()); - ukf_wrapper_ = std::make_unique( - localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter" - ); - ukf_wrapper_->initialize(); - localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); - localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_); + // Detect which filters have parameters configured by checking + // parameter overrides (loaded from YAML before declaration) + const std::string global_prefix = plugin_name + ".global_filter."; + const std::string local_prefix = plugin_name + ".local_filter."; + + const auto & overrides = + localizer_node->get_node_parameters_interface()->get_parameter_overrides(); + + for (const auto & [key, _] : overrides) { + if (!has_global_filter_ && key.rfind(global_prefix, 0) == 0) { + has_global_filter_ = true; + } + if (!has_local_filter_ && key.rfind(local_prefix, 0) == 0) { + has_local_filter_ = true; + } + if (has_global_filter_ && has_local_filter_) { + break; + } + } + + if (!has_global_filter_ && !has_local_filter_) { + RCLCPP_FATAL( + localizer_node->get_logger(), + "No parameters found for either '%s' or '%s'. " + "At least one filter must be configured.", + global_prefix.c_str(), local_prefix.c_str()); + throw std::runtime_error( + "FusionLocalizer: no global_filter or local_filter parameters detected. " + "At least one filter must be configured."); + } + + if (has_global_filter_) { + ukf_global_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false + ); + ukf_global_->initialize(); + } else { + RCLCPP_WARN(localizer_node->get_logger(), + "No global_filter parameters found. Global filter will NOT be created."); + } + + if (has_local_filter_) { + ukf_local_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true + ); + ukf_local_->initialize(); + } else { + RCLCPP_WARN(localizer_node->get_logger(), + "No local_filter parameters found. Local filter will NOT be created."); + } + + // GPS-related setup only needed when global filter is active + if (has_global_filter_) { + 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 + ".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_); + + localizer_node->declare_parameter( + plugin_name + ".navsatfix_topic", std::string("gps/filtered")); + localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); + navsat_pub_ = localizer_node->create_publisher( + navsatfix_topic_, rclcpp::QoS(10)); + } - localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0)); - localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_); } catch (const std::exception & e) { RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", e.what()); - // Raise error throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what()); } + if (has_global_filter_) { + GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_, + UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); + UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S"); + UTM_origin_z_ = altitude_origin_; - 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()); + n_gps_sensors_ = static_cast(ukf_global_->getGpsCallbackDataArr().size()); + last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + } RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); } -// 2. Hook de actualización RT (Tu "Timer" de alta frecuencia) void FusionLocalizer::update_rt(NavState & nav_state) { - if(n_gps_sensors_ && nav_state.has("gnss")) { - auto gps_data = nav_state.get(std::string("gnss")); - for (int i = 0; i < n_gps_sensors_; ++i) { - double gps_time = gps_data[i]->data.header.stamp.sec + - gps_data[i]->data.header.stamp.nanosec * 1e-9; - if (gps_time > last_gps_stamp_[i]) { - // if(true) { - last_gps_stamp_[i] = gps_time; - auto pose = navsatfix_to_pose(gps_data[i]->data); - // nav_state.set("UTM_gnss_pose", pose); - // Call the wrapper callback - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - ukf_wrapper_->poseCallback( - std::make_shared(pose), - ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data - tf_info.map_frame, // target_frame - tf_info.odom_frame, // pose_source_frame - false // imu_data - ); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + if (has_global_filter_) { + if (n_gps_sensors_ && nav_state.has("gnss")) { + auto gps_data = nav_state.get(std::string("gnss")); + const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); + for (int i = 0; i < n_gps_sensors_; ++i) { + if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + continue; + } + rclcpp::Time gps_time(gps_data[i]->data.header.stamp); + if (gps_time > last_gps_stamp_[i]) { + EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); + last_gps_stamp_[i] = gps_time; + auto pose = + std::make_shared(navsatfix_to_pose( + gps_data[i]->data)); + if (!first_pose_received_) { + RCLCPP_INFO(get_node()->get_logger(), + "First valid GPS fix received. Initializing filter state."); + if(nav_state.has("imu")) { + auto imu_data = nav_state.get(std::string("imu")); + if (!imu_data.empty()) { + pose->pose.pose.orientation = imu_data[0]->data.orientation; + } + } + ukf_global_->setPoseCallback(pose); + first_pose_received_ = true; + continue; + } + ukf_global_->poseCallback( + pose, + gps_cb_arr[i], + tf_info.map_frame, + gps_data[i]->data.header.frame_id, + false + ); + } } } + + ukf_global_->periodicUpdate(); + + nav_msgs::msg::Odometry global_odom; + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + nav_state.set("robot_pose", global_odom); + navsat_pub_->publish(odom_to_navsatfix(global_odom)); + } } - ukf_wrapper_->periodicUpdate(); + if (has_local_filter_) { + ukf_local_->periodicUpdate(); - nav_msgs::msg::Odometry current_odom; - if (ukf_wrapper_->getFilteredOdometryMessage(¤t_odom)) { - nav_state.set("robot_pose", current_odom); + nav_msgs::msg::Odometry local_odom; + if (ukf_local_->getFilteredOdometryMessage(&local_odom)) { + nav_state.set("robot_pose_local", local_odom); + } } } -// 3. Hook de actualización no-RT (baja frecuencia) void FusionLocalizer::update([[maybe_unused]] NavState & nav_state) { @@ -110,16 +191,12 @@ 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; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); pose_msg.header.frame_id = tf_info.map_frame; - // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y) double utm_x, utm_y; int zone; bool northp; @@ -130,7 +207,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose zone, northp, utm_x, - utm_y); + utm_y, + UTM_zone_number_); pose_msg.pose.pose.position.x = utm_x - UTM_origin_x_; pose_msg.pose.pose.position.y = utm_y - UTM_origin_y_; @@ -143,23 +221,65 @@ 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 + double default_var = 1.0; // 1 meter variance standard + + if (navsat_msg.position_covariance_type == + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) + { + pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0]; + pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4]; + pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; + + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; + } else { + // Fallback variances if GPS doesn't provide them + pose_msg.pose.covariance[0] = default_var; + pose_msg.pose.covariance[7] = default_var; + pose_msg.pose.covariance[14] = default_var; + + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; + + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, + "NavSatFix covariance type unknown or invalid. Using default covariance."); + } - 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 + return pose_msg; +} - 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 +sensor_msgs::msg::NavSatFix FusionLocalizer::odom_to_navsatfix( + const nav_msgs::msg::Odometry & odom_msg) +{ + sensor_msgs::msg::NavSatFix navsat_msg; - pose_msg.pose.covariance[21] = 99999.0; - pose_msg.pose.covariance[28] = 99999.0; - pose_msg.pose.covariance[35] = 99999.0; + navsat_msg.header = odom_msg.header; + navsat_msg.header.frame_id = "gps_link"; + navsat_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + navsat_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - return pose_msg; + const double utm_x = odom_msg.pose.pose.position.x + UTM_origin_x_; + const double utm_y = odom_msg.pose.pose.position.y + UTM_origin_y_; + + double latitude = 0.0; + double longitude = 0.0; + GeographicLib::UTMUPS::Reverse( + UTM_zone_number_, UTM_zone_northp_, utm_x, utm_y, latitude, longitude); + + navsat_msg.latitude = latitude; + navsat_msg.longitude = longitude; + navsat_msg.altitude = odom_msg.pose.pose.position.z + UTM_origin_z_; + + navsat_msg.position_covariance[0] = odom_msg.pose.covariance[0]; + navsat_msg.position_covariance[4] = odom_msg.pose.covariance[7]; + navsat_msg.position_covariance[8] = odom_msg.pose.covariance[14]; + + navsat_msg.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + return navsat_msg; } } // namespace easynav 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..ebd38f08 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 @@ -75,7 +75,6 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include -#include #include "easynav_common/RTTFBuffer.hpp" @@ -86,7 +85,8 @@ using namespace std::chrono_literals; UkfWrapper::UkfWrapper( std::shared_ptr parent_node, const std::string & tf_prefix, - const std::string & plugin_name) + const std::string & plugin_name, + bool local_filter) : print_diagnostics_(true), publish_acceleration_(false), publish_transform_(true), @@ -114,14 +114,15 @@ UkfWrapper::UkfWrapper( last_set_pose_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0, 0, RCL_ROS_TIME), tf_timeout_(0ns), - tf_time_offset_(0ns) + tf_time_offset_(0ns), + local_filter_(local_filter) { 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_); + // Use the shared RTTFBuffer singleton instead of creating a private tf2_ros::Buffer + tf_buffer_ = easynav::RTTFBuffer::getInstance(); state_variable_names_.push_back("X"); state_variable_names_.push_back("Y"); @@ -147,8 +148,6 @@ UkfWrapper::~UkfWrapper() 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(); @@ -184,8 +183,7 @@ void UkfWrapper::reset() last_diff_time_ = parent_node_->now().seconds(); - // clear tf buffer to avoid TF_OLD_DATA errors - tf_buffer_->clear(); + // Note: tf_buffer_ is the shared RTTFBuffer singleton, do not clear it here // clear last message timestamp, so older messages will be accepted last_message_times_.clear(); @@ -370,7 +368,7 @@ void UkfWrapper::enqueueMeasurement( const std::vector & update_vector, const double mahalanobis_thresh, const rclcpp::Time & time) { - MeasurementPtr meas = MeasurementPtr(new Measurement()); + MeasurementPtr meas = std::make_shared(); meas->topic_name_ = topic_name; meas->measurement_ = measurement; @@ -895,7 +893,11 @@ 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; + if(local_filter_) { + world_frame_id_ = tf_info.odom_frame; + } else { + world_frame_id_ = tf_info.map_frame; + } base_link_output_frame_id_ = base_link_frame_id_; @@ -1172,30 +1174,39 @@ void UkfWrapper::loadParams() "\npermit_corrected_publication is " << permit_corrected_publication_ << "\nprint_diagnostics is " << print_diagnostics_ << "\n"); + // Determine mode prefix for service/topic names to avoid collisions + // between local and global filter instances + std::string mode_prefix; + if (local_filter_) { + mode_prefix = "local_"; + } else { + mode_prefix = "global_"; + } + // Create a subscriber for manually setting/resetting pose set_pose_sub_ = parent_node_->create_subscription( - "set_pose", rclcpp::QoS(1), + mode_prefix + "set_pose", rclcpp::QoS(1), std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1), options); // Create a service for manually setting/resetting pose set_pose_service_ = parent_node_->create_service( - "set_pose", std::bind( + mode_prefix + "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( + "~/" + mode_prefix + "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( + mode_prefix + "reset", std::bind( &UkfWrapper::resetSrvCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -1203,7 +1214,7 @@ void UkfWrapper::loadParams() // publishing toggle_filter_processing_srv_ = parent_node_->create_service( - "~/toggle", std::bind( + "~/" + mode_prefix + "toggle", std::bind( &UkfWrapper::toggleFilterProcessingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -2090,6 +2101,7 @@ void UkfWrapper::odometryCallback( 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 { @@ -2262,15 +2274,21 @@ void UkfWrapper::initialize() // Position publisher rclcpp::PublisherOptions publisher_options; publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + std::string mode_prefix; + if (local_filter_) { + mode_prefix = "local_"; + } else { + mode_prefix = "global_"; + } position_pub_ = parent_node_->create_publisher( - "odometry/filtered", rclcpp::QoS(10), publisher_options); + mode_prefix + "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); + mode_prefix + "accel/filtered", rclcpp::QoS(10), publisher_options); } // Block commented as it is handled in the update_rt @@ -2864,7 +2882,7 @@ bool UkfWrapper::prepareAcceleration( // 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_, + tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_, target_frame_trans); if (can_transform) { @@ -3093,7 +3111,7 @@ bool UkfWrapper::preparePose( // 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_, + tf_buffer_, final_target_frame, pose_tmp.frame_id_, rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, target_frame_trans); @@ -3103,7 +3121,7 @@ bool UkfWrapper::preparePose( 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_, + tf_buffer_, source_frame, base_link_frame_id_, rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, source_frame_trans); } @@ -3525,7 +3543,7 @@ bool UkfWrapper::prepareTwist( // 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_, + tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_, target_frame_trans); if (can_transform) { From 2d0b6d7c7850d265865b67d3aa066231e067c222 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Tue, 31 Mar 2026 19:01:02 +0200 Subject: [PATCH 174/230] Update plugins to new sensors API --- .../include/easynav_mpc_controller/MPCController.hpp | 2 +- .../src/easynav_mpc_controller/MPCController.cpp | 4 ++-- .../src/easynav_mppi_controller/MPPIController.cpp | 4 ++-- .../src/easynav_serest_controller/SerestController.cpp | 6 +++--- .../src/easynav_vff_controller/VffController.cpp | 4 ++-- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 6 +++--- .../src/easynav_fusion_localizer/FusionLocalizer.cpp | 4 ++-- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 8 ++++---- .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 6 +++--- .../filters/ObstacleFilter.cpp | 6 +++--- .../tests/costmap_mapsmanager_tests.cpp | 2 +- maps_managers/easynav_navmap_maps_manager/README.md | 2 +- .../filters/ObstacleFilter.cpp | 6 +++--- .../tests/navmap_mapsmanager_tests.cpp | 2 +- .../filters/InflationFilter.cpp | 4 ++-- .../filters/ObstacleFilter.cpp | 8 ++++---- .../easynav_simple_maps_manager/SimpleMapsManager.cpp | 10 +++++++--- .../tests/simple_mapsmanager_tests.cpp | 10 +++++----- 18 files changed, 49 insertions(+), 45 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 ebb7fb6f..40e8472d 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -37,7 +37,7 @@ #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "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 36116946..07487393 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -142,7 +142,7 @@ MPCController::update_rt(NavState & nav_state) } } - if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("points")) { + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has_group("points")) { if(verbose_) { std::cout << "No Path, No Points or No Robot Pose" << std::endl; } @@ -209,7 +209,7 @@ 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 & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) 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 c0fc3471..5823981e 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -17,7 +17,7 @@ /// \brief Implementation of the MPPIController class. #include "easynav_mppi_controller/MPPIController.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_system/GoalManager.hpp" @@ -182,7 +182,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 & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) 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 659dd4e6..a15bad53 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,7 @@ #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_serest_controller/SerestController.hpp" @@ -295,9 +295,9 @@ SerestController::closest_obstacle_distance( } // 2) Analyze distance sensors - if (!nav_state.has("points")) {return std::numeric_limits::infinity();} + if (!nav_state.has_group("points")) {return std::numeric_limits::infinity();} - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto view = PointPerceptionsOpsView(perceptions); 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 65d5e626..16ce5020 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -18,7 +18,7 @@ #include "easynav_vff_controller/VffController.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/goals.hpp" @@ -252,7 +252,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_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto fused = 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 30c3bbbb..619e1d6f 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -22,7 +22,7 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -566,12 +566,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("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_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 7982a9a7..2bdf750f 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -4,8 +4,8 @@ #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/IMUPerception.hpp" -#include "easynav_common/types/GNSSPerception.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" +#include "easynav_sensors/types/GNSSPerception.hpp" #include "sensor_msgs/msg/nav_sat_status.hpp" #include 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 79117a91..0b127310 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -34,8 +34,8 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/types/IMUPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" #include "navmap_core/NavMap.hpp" @@ -689,11 +689,11 @@ static ScoreAgg score_particle_sensor_cloud( // ---------- correct() ---------- void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet"); return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); if (!nav_state.has("map.bonxai")) { RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet"); 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 6fc0b92a..2c50681c 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -22,7 +22,7 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_simple_localizer/AMCLLocalizer.hpp" @@ -368,12 +368,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("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_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 b354f31e..36a3dd26 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,7 @@ #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -41,11 +41,11 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(NavState & nav_state) { - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); Costmap2D & dynamic_map = *dynamic_map_ptr; 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 f14df242..8440f0c2 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 @@ -15,7 +15,7 @@ #include -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 9f925d2f..f019a211 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -59,7 +59,7 @@ All parameters are declared under the plugin namespace, i.e. | `.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"`. | +| **Input Key:** | | | Reads list of point clouds from `NavState` group `"points"`. | | **Output Layer:** | | | Updates or creates NavMap layer `"obstacles"`. | #### **InflationFilter** 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 aeeed492..a031c277 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,7 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" @@ -44,9 +44,9 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { if (!nav_state.has("map.navmap")) {return;} - if (!nav_state.has("points")) {return;} + if (!nav_state.has_group("points")) {return;} - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 d06b8904..4c55b8fe 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 @@ -15,7 +15,7 @@ #include -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" 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 0703a15f..09a13363 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 @@ -42,8 +42,8 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/Perceptions.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "octomap_core/Octomap.hpp" 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 33a1abb9..987df0c3 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 @@ -17,8 +17,8 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/Perceptions.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "octomap_core/Octomap.hpp" @@ -45,11 +45,11 @@ ObstacleFilter::update(::easynav::NavState & nav_state) if (!nav_state.has("map")) { return; } - if (!nav_state.has("points")) { + if (!nav_state.has_group("points")) { return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); octomap_ = nav_state.get<::octomap::Octomap>("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 9241f3ce..446954e7 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 @@ -17,7 +17,7 @@ /// \brief Implementation of the SimpleMapsManager class. #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" @@ -138,13 +138,17 @@ SimpleMapsManager::update(NavState & nav_state) dynamic_map_.deep_copy(static_map_); - if (!nav_state.has("points")) { + std::cout << "nav_state.has_group(points): " << nav_state.has_group("points") << std::endl; + if (!nav_state.has_group("points")) { nav_state.set("map.static", static_map_); nav_state.set("map.dynamic", dynamic_map_); return; } - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_group("points"); + std::cout << "perceptions size: " << perceptions.size() << std::endl; + std::cout << "perception[0] size: " << perceptions[0]->data.points.size() << std::endl; + std::cout << "perception[0] valid: " << perceptions[0]->valid << std::endl; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); rclcpp::Time stamp; 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 82c67758..74873bc0 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 @@ -17,7 +17,7 @@ #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "rclcpp/rclcpp.hpp" @@ -68,7 +68,8 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) easynav::NavState navstate; auto perception = std::make_shared(); - navstate.set("points", easynav::PointPerceptions()); + navstate.set("laser", easynav::PointPerception()); + navstate.set_group("points", {"laser"}); perception->data.points.resize(6); perception->data.points[0].x = 1.0; @@ -94,9 +95,8 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) perception->frame_id = "map"; perception->valid = true; - easynav::PointPerceptions perceptions; - perceptions.push_back(perception); - navstate.set("points", perceptions); + navstate.set("laser", perception); + // navstate.set_group("points", {"laser"}); manager->update(navstate); From 21bef9d60312caaac26d6cddd6348d7be099989f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 3 Apr 2026 09:52:42 +0200 Subject: [PATCH 175/230] Adaptations to #94 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_mpc_controller/MPCController.cpp | 6 +++--- .../src/easynav_mppi_controller/MPPIController.cpp | 2 +- .../easynav_serest_controller/SerestController.cpp | 2 +- .../src/easynav_vff_controller/VffController.cpp | 2 +- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 7 +++---- .../src/easynav_fusion_localizer/FusionLocalizer.cpp | 8 +++----- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 11 +++++------ .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 7 +++---- .../filters/ObstacleFilter.cpp | 6 +++--- .../filters/ObstacleFilter.cpp | 8 ++++++-- .../filters/ObstacleFilter.cpp | 6 ++++-- .../easynav_simple_maps_manager/SimpleMapsManager.cpp | 9 ++++++--- 12 files changed, 39 insertions(+), 35 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 07487393..ac825d4c 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -142,7 +142,9 @@ MPCController::update_rt(NavState & nav_state) } } - if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has_group("points")) { + const auto & perceptions = nav_state.get_no_group(); + + if (!nav_state.has("path") || !nav_state.has("robot_pose") || perceptions.empty()) { if(verbose_) { std::cout << "No Path, No Points or No Robot Pose" << std::endl; } @@ -208,8 +210,6 @@ MPCController::update_rt(NavState & nav_state) local_horizon = num_elements - 1; } const auto & last_pose = path.poses[local_horizon].pose.position; - - const auto & perceptions = nav_state.get_group("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) 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 5823981e..f80ca7ff 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -182,7 +182,7 @@ MPPIController::update_rt(NavState & nav_state) } const auto & pose = nav_state.get("robot_pose").pose.pose; - const auto & perceptions = nav_state.get_group("points"); + const auto & perceptions = nav_state.get_no_group(); 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}) 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 a15bad53..9a32f1ff 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -297,7 +297,7 @@ SerestController::closest_obstacle_distance( // 2) Analyze distance sensors if (!nav_state.has_group("points")) {return std::numeric_limits::infinity();} - const auto & perceptions = nav_state.get_group("points"); + const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto view = PointPerceptionsOpsView(perceptions); 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 16ce5020..1fe63e46 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -252,7 +252,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_group("points"); + const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto fused = 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 619e1d6f..7c26a2c0 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -566,13 +566,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has_group("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); - if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); return; 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 2bdf750f..f1cd27f5 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -142,11 +142,9 @@ void FusionLocalizer::update_rt(NavState & nav_state) if (!first_pose_received_) { RCLCPP_INFO(get_node()->get_logger(), "First valid GPS fix received. Initializing filter state."); - if(nav_state.has("imu")) { - auto imu_data = nav_state.get(std::string("imu")); - if (!imu_data.empty()) { - pose->pose.pose.orientation = imu_data[0]->data.orientation; - } + if (nav_state.has("imu")) { + auto imu_data = nav_state.get(std::string("imu")); + pose->pose.pose.orientation = imu_data.data.orientation; } ukf_global_->setPoseCallback(pose); first_pose_received_ = true; 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 0b127310..0371eab1 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -496,9 +496,8 @@ 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"); - if (imus.empty() || !imus.back()) {return std::nullopt;} - const auto & imu_msg = imus.back()->data; + const auto & imu = nav_state.get("imu"); + const auto & imu_msg = imu.data; tf2::Quaternion q(imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w); if (q.length2() < 1e-12) {return std::nullopt;} @@ -689,11 +688,11 @@ static ScoreAgg score_particle_sensor_cloud( // ---------- correct() ---------- void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has_group("points")) { - RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); if (!nav_state.has("map.bonxai")) { RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet"); 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 2c50681c..42165823 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -368,13 +368,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has_group("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); - if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); return; 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 36a3dd26..69e41672 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 @@ -41,12 +41,12 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(NavState & nav_state) { - if (!nav_state.has_group("points")) { + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); - 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(); 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 a031c277..87532a09 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 @@ -44,9 +44,13 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { if (!nav_state.has("map.navmap")) {return;} - if (!nav_state.has_group("points")) {return;} - const auto & perceptions = nav_state.get_group("points"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); + return; + } + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 987df0c3..105ab371 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 @@ -45,11 +45,13 @@ ObstacleFilter::update(::easynav::NavState & nav_state) if (!nav_state.has("map")) { return; } - if (!nav_state.has_group("points")) { + + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get_group("points"); octomap_ = nav_state.get<::octomap::Octomap>("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 446954e7..75691590 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 @@ -138,14 +138,17 @@ SimpleMapsManager::update(NavState & nav_state) dynamic_map_.deep_copy(static_map_); - std::cout << "nav_state.has_group(points): " << nav_state.has_group("points") << std::endl; - if (!nav_state.has_group("points")) { + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); + } + + if (perceptions.empty()) { nav_state.set("map.static", static_map_); nav_state.set("map.dynamic", dynamic_map_); return; } - const auto & perceptions = nav_state.get_group("points"); std::cout << "perceptions size: " << perceptions.size() << std::endl; std::cout << "perception[0] size: " << perceptions[0]->data.points.size() << std::endl; std::cout << "perception[0] valid: " << perceptions[0]->valid << std::endl; From cca59aa65552cb7d3c88356e18502910bd0a75fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 3 Apr 2026 11:18:16 +0200 Subject: [PATCH 176/230] Remove group points in tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../tests/simple_mapsmanager_tests.cpp | 1 - 1 file changed, 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 74873bc0..e6adb415 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,6 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) easynav::NavState navstate; auto perception = std::make_shared(); navstate.set("laser", easynav::PointPerception()); - navstate.set_group("points", {"laser"}); perception->data.points.resize(6); perception->data.points[0].x = 1.0; From 7ddb356c07f3b5b98e4b4266b47f9b3ad4fef876 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Apr 2026 07:23:06 +0200 Subject: [PATCH 177/230] Reset pose from RViz2 for every localizer 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/CMakeLists.txt | 2 +- .../tests/costmap_localizer_tests.cpp | 252 +++++++++------- .../easynav_fusion_localizer/CMakeLists.txt | 3 + .../FusionLocalizer.hpp | 7 + .../FusionLocalizer.cpp | 54 ++++ .../easynav_fusion_localizer/ukf_wrapper.cpp | 23 +- .../tests/CMakeLists.txt | 12 + .../tests/fusion_localizer_tests.cpp | 183 ++++++++++++ .../easynav_gps_localizer/CMakeLists.txt | 3 + .../easynav_gps_localizer/GpsLocalizer.hpp | 12 + .../easynav_gps_localizer/GpsLocalizer.cpp | 50 ++++ .../tests/CMakeLists.txt | 10 + .../tests/gps_localizer_tests.cpp | 186 ++++++++++++ .../easynav_navmap_localizer/CMakeLists.txt | 2 +- .../AMCLLocalizer.hpp | 10 + .../AMCLLocalizer.cpp | 154 +++++++++- .../tests/CMakeLists.txt | 4 +- .../tests/costmap_localizer_tests.cpp | 157 ---------- .../tests/navmap_localizer_tests.cpp | 182 ++++++++++++ .../easynav_simple_localizer/CMakeLists.txt | 2 +- .../AMCLLocalizer.hpp | 10 + .../AMCLLocalizer.cpp | 155 +++++++++- .../tests/simple_localizer_tests.cpp | 280 +++++++++--------- .../SimpleMapsManager.cpp | 3 - 24 files changed, 1331 insertions(+), 425 deletions(-) create mode 100644 localizers/easynav_fusion_localizer/tests/CMakeLists.txt create mode 100644 localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp create mode 100644 localizers/easynav_gps_localizer/tests/CMakeLists.txt create mode 100644 localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp delete mode 100644 localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp create mode 100644 localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index aabb1b11..15c2642b 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -58,7 +58,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index bc03cbe6..6259a598 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -15,143 +15,169 @@ #include +#include +#include +#include +#include +#include + #include "easynav_costmap_localizer/AMCLLocalizer.hpp" -#include "easynav_costmap_common/costmap_2d.hpp" +#include "easynav_localizer/LocalizerNode.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test +namespace { -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) { - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; } -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) +double yaw_from_tf(const tf2::Transform & tf) { - 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()); - - 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)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; } -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { +class FriendAMCLLocalizer : public easynav::AMCLLocalizer +{ public: - void force_path(const std::string & path) {map_path_ = path;} + using easynav::AMCLLocalizer::init_pose_sub_; }; -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) +class AMCLLocalizerInitialPoseTest : public ::testing::Test { - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); +} // namespace - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.25; + const double y0 = -2.5; + const double yaw0 = 0.4; + + const double x1 = -0.75; + const double y1 = 0.9; + const double yaw1 = -1.2; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.compute_odom_from_tf", true), + }); + + auto node = std::make_shared(options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } } diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index 94db9550..c0755cd4 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -81,6 +81,9 @@ if(BUILD_TESTING) 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) 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 1f49c3f8..740a03df 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -27,6 +27,13 @@ class FusionLocalizer : public easynav::LocalizerMethodBase FusionLocalizer() = default; virtual ~FusionLocalizer() = default; +protected: + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + + /// Callback for /initialpose. + void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + protected: /** * @brief Hook de inicialización de MethodBase. 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 f1cd27f5..4f2fcdbb 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -12,6 +12,9 @@ #include "easynav_common/YTSession.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + namespace easynav { @@ -22,6 +25,11 @@ void FusionLocalizer::on_initialize() auto node = get_node(); + // Subscribe to initial pose + init_pose_sub_ = node->create_subscription( + "initialpose", 10, + std::bind(&FusionLocalizer::init_pose_callback, this, std::placeholders::_1)); + auto localizer_node = std::dynamic_pointer_cast(node); const std::string & plugin_name = this->get_plugin_name(); @@ -100,6 +108,39 @@ void FusionLocalizer::on_initialize() navsatfix_topic_, rclcpp::QoS(10)); } + // Optional initial pose from parameters (kept consistent with AMCL parameter names) + localizer_node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); + localizer_node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); + localizer_node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); + + double init_x = 0.0; + double init_y = 0.0; + double init_yaw = 0.0; + localizer_node->get_parameter(plugin_name + ".initial_pose.x", init_x); + localizer_node->get_parameter(plugin_name + ".initial_pose.y", init_y); + localizer_node->get_parameter(plugin_name + ".initial_pose.yaw", init_yaw); + + if (std::abs(init_x) > 1e-12 || std::abs(init_y) > 1e-12 || std::abs(init_yaw) > 1e-12) { + auto init_pose = std::make_shared(); + init_pose->header.stamp = localizer_node->now(); + init_pose->header.frame_id = tf_info.map_frame; + init_pose->pose.pose.position.x = init_x; + init_pose->pose.pose.position.y = init_y; + init_pose->pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, init_yaw); + init_pose->pose.pose.orientation = tf2::toMsg(q); + init_pose->pose.covariance.fill(0.0); + + if (ukf_global_) { + ukf_global_->setPoseCallback(init_pose); + first_pose_received_ = true; + } + if (ukf_local_) { + ukf_local_->setPoseCallback(init_pose); + } + } + } catch (const std::exception & e) { RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", @@ -120,6 +161,19 @@ void FusionLocalizer::on_initialize() RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); } +void FusionLocalizer::init_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + // Forward initial pose to any active filters + if (ukf_global_) { + ukf_global_->setPoseCallback(msg); + first_pose_received_ = true; + } + if (ukf_local_) { + ukf_local_->setPoseCallback(msg); + } +} + void FusionLocalizer::update_rt(NavState & nav_state) { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 6164b4de..5ce5bd30 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 @@ -2538,10 +2538,29 @@ void UkfWrapper::setPoseCallback( update_vector, measurement, measurement_covariance); // For the state + const auto now = parent_node_->now(); + + // Deterministically set the internal state to the requested pose. + // This ensures a /set_pose (or /initialpose) immediately changes outputs. filter_.setState(measurement); filter_.setEstimateErrorCovariance(measurement_covariance); - - filter_.setLastMeasurementTime(parent_node_->now()); + filter_.setLastMeasurementTime(now); + + // If the filter was not initialized yet, feed an equivalent measurement + // through the filter to flip its initialized flag. + if (!filter_.getInitializedStatus()) { + Measurement pose_measurement; + pose_measurement.topic_name_ = topic_name; + pose_measurement.measurement_ = measurement; + pose_measurement.covariance_ = measurement_covariance; + pose_measurement.update_vector_ = update_vector; + pose_measurement.time_ = now; + pose_measurement.mahalanobis_thresh_ = std::numeric_limits::max(); + pose_measurement.latest_control_ = latest_control_; + pose_measurement.latest_control_time_ = latest_control_time_; + + filter_.processMeasurement(pose_measurement); + } RF_DEBUG("\n------ /UkfWrapper::setPoseCallback ------\n"); } diff --git a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt new file mode 100644 index 00000000..8d470925 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt @@ -0,0 +1,12 @@ +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(easynav_localizer REQUIRED) + +ament_add_gtest(fusion_localizer_tests fusion_localizer_tests.cpp) + +target_link_libraries(fusion_localizer_tests + fusion_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + easynav_localizer::easynav_localizer +) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp new file mode 100644 index 00000000..1cbac580 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp @@ -0,0 +1,183 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 + +#include +#include +#include +#include +#include + +#include "easynav_fusion_localizer/FusionLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" + +#include "easynav_common/types/NavState.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ + +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; +} + +class FriendFusionLocalizer : public easynav::FusionLocalizer +{ +public: + using easynav::FusionLocalizer::init_pose_sub_; + using easynav::FusionLocalizer::update_rt; +}; + +class FusionLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.5; + const double y0 = -0.25; + const double yaw0 = 0.7; + + const double x1 = -0.8; + const double y1 = 2.1; + const double yaw1 = -1.2; + + // Provide at least one parameter override so FusionLocalizer considers a filter configured. + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.global_filter.frequency", 30.0), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + }); + + auto node = std::make_shared(options); + node->declare_parameter>("localizer_types", std::vector{}); + + auto localizer = std::make_shared(); + + ASSERT_NO_THROW(localizer->initialize(node, "test")); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("fusion_test_pub_node"); + auto initialpose_pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < connect_deadline && + initialpose_pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + easynav::NavState nav_state; + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update_rt(nav_state); + if (nav_state.has("robot_pose")) { + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x0) < 1e-6 && + std::abs(odom.pose.pose.position.y - y0) < 1e-6 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw0) < 1e-3; + if (ok) { + break; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + ASSERT_TRUE(nav_state.has("robot_pose")); + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-3); + } + + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_msg; + init_pose_msg.header.stamp = pub_node->now(); + init_pose_msg.header.frame_id = "map"; + init_pose_msg.pose.pose.position.x = x1; + init_pose_msg.pose.pose.position.y = y1; + init_pose_msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + init_pose_msg.pose.pose.orientation = tf2::toMsg(q); + init_pose_msg.pose.covariance.fill(0.0); + initialpose_pub->publish(init_pose_msg); + + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update_rt(nav_state); + + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x1) < 1e-6 && + std::abs(odom.pose.pose.position.y - y1) < 1e-6 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-3; + + if (ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-3); + } +} diff --git a/localizers/easynav_gps_localizer/CMakeLists.txt b/localizers/easynav_gps_localizer/CMakeLists.txt index f18114bc..b44c29b4 100644 --- a/localizers/easynav_gps_localizer/CMakeLists.txt +++ b/localizers/easynav_gps_localizer/CMakeLists.txt @@ -59,6 +59,9 @@ if(BUILD_TESTING) 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) 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 03d1fcdc..2628d6a9 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -21,10 +21,12 @@ #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "easynav_core/LocalizerMethodBase.hpp" #include "tf2_ros/static_transform_broadcaster.hpp" #include "sensor_msgs/msg/imu.hpp" #include +#include namespace easynav { @@ -83,6 +85,16 @@ class GpsLocalizer : public easynav::LocalizerMethodBase */ virtual void update(NavState & nav_state) override; +protected: + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + + /// Initial pose received but not yet applied (e.g. waiting for a valid GPS fix). + std::optional pending_init_pose_; + + /// Callback for /initialpose. + void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + private: /** * @brief Internal representation of the robot's current odometry. 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 c4eec332..a0ab2cf9 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -20,12 +20,16 @@ #include "easynav_common/RTTFBuffer.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + namespace easynav { void GpsLocalizer::on_initialize() { auto node = get_node(); + const auto & plugin_name = get_plugin_name(); // Initialize the odometry message odom_.header.stamp = get_node()->now(); @@ -46,6 +50,38 @@ void GpsLocalizer::on_initialize() "imu/data", rclcpp::SensorDataQoS().reliable(), std::bind(&GpsLocalizer::imu_callback, this, std::placeholders::_1)); + // Subscribe to initial pose + init_pose_sub_ = node->create_subscription( + "initialpose", 10, + std::bind(&GpsLocalizer::init_pose_callback, this, std::placeholders::_1)); + + // Optional initial pose from parameters (kept consistent with AMCL parameter names) + node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); + + double init_x = 0.0; + double init_y = 0.0; + double init_yaw = 0.0; + node->get_parameter(plugin_name + ".initial_pose.x", init_x); + node->get_parameter(plugin_name + ".initial_pose.y", init_y); + node->get_parameter(plugin_name + ".initial_pose.yaw", init_yaw); + + if (std::abs(init_x) > 1e-12 || std::abs(init_y) > 1e-12 || std::abs(init_yaw) > 1e-12) { + geometry_msgs::msg::PoseWithCovarianceStamped init_pose; + init_pose.header.stamp = node->now(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + init_pose.header.frame_id = tf_info.map_frame; + init_pose.pose.pose.position.x = init_x; + init_pose.pose.pose.position.y = init_y; + init_pose.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, init_yaw); + init_pose.pose.pose.orientation = tf2::toMsg(q); + init_pose.pose.covariance.fill(0.0); + pending_init_pose_ = init_pose; + } + // Create publisher odom_pub_ = node->create_publisher( "robot/odom_gps", rclcpp::SensorDataQoS().reliable()); @@ -81,6 +117,12 @@ void GpsLocalizer::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) imu_msg_ = std::move(*msg); } +void GpsLocalizer::init_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + pending_init_pose_ = *msg; +} + void GpsLocalizer::update_rt([[maybe_unused]] NavState & nav_state) { @@ -98,6 +140,14 @@ void GpsLocalizer::update(NavState & nav_state) GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, utm_x, utm_y); std::string utm_zone = std::to_string(zone) + (northp ? "N" : "S"); + // If an initial pose was provided, align the UTM origin so that the current + // GPS fix maps to that pose in the map frame. + if (pending_init_pose_.has_value() && gps_msg_ != sensor_msgs::msg::NavSatFix()) { + origin_utm_.x = utm_x - pending_init_pose_->pose.pose.position.x; + origin_utm_.y = utm_y - pending_init_pose_->pose.pose.position.y; + pending_init_pose_.reset(); + } + if (origin_utm_ == geometry_msgs::msg::Point() && gps_msg_ != sensor_msgs::msg::NavSatFix()) { diff --git a/localizers/easynav_gps_localizer/tests/CMakeLists.txt b/localizers/easynav_gps_localizer/tests/CMakeLists.txt new file mode 100644 index 00000000..0e8b0048 --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/CMakeLists.txt @@ -0,0 +1,10 @@ +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +ament_add_gtest(gps_localizer_tests gps_localizer_tests.cpp) + +target_link_libraries(gps_localizer_tests + gps_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp new file mode 100644 index 00000000..b594eaee --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp @@ -0,0 +1,186 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 + +#include +#include +#include +#include + +#include "easynav_gps_localizer/GpsLocalizer.hpp" + +#include "easynav_common/types/NavState.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +namespace +{ + +class FriendGpsLocalizer : public easynav::GpsLocalizer +{ +public: + using easynav::GpsLocalizer::init_pose_sub_; + using easynav::GpsLocalizer::update; +}; + +class GpsLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(GpsLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.1; + const double y0 = -0.7; + + const double x1 = -2.3; + const double y1 = 0.4; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", 0.0), + }); + + auto node = std::make_shared("test_gps_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("gps_test_pub_node"); + auto initialpose_pub = pub_node->create_publisher( + "initialpose", 10); + auto gps_pub = pub_node->create_publisher( + "robot/gps/fix", rclcpp::SensorDataQoS().reliable()); + auto imu_pub = pub_node->create_publisher( + "imu/data", rclcpp::SensorDataQoS().reliable()); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < connect_deadline && + (initialpose_pub->get_subscription_count() == 0 || + gps_pub->get_subscription_count() == 0 || + imu_pub->get_subscription_count() == 0)) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + sensor_msgs::msg::Imu imu_msg; + imu_msg.header.stamp = pub_node->now(); + imu_msg.header.frame_id = ""; + imu_msg.angular_velocity.z = 0.0; + imu_msg.orientation.w = 1.0; + imu_pub->publish(imu_msg); + + sensor_msgs::msg::NavSatFix gps_msg; + gps_msg.header.stamp = pub_node->now(); + gps_msg.header.frame_id = ""; + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + gps_msg.latitude = 42.0; + gps_msg.longitude = -1.0; + gps_msg.altitude = 0.0; + gps_pub->publish(gps_msg); + + easynav::NavState nav_state; + + // Deliver GPS/IMU callbacks and run update() until the initial pose is applied. + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update(nav_state); + + if (nav_state.has("robot_pose")) { + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x0) < 1e-9 && + std::abs(odom.pose.pose.position.y - y0) < 1e-9; + if (ok) { + break; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + ASSERT_TRUE(nav_state.has("robot_pose")); + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + } + + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_msg; + init_pose_msg.header.stamp = pub_node->now(); + init_pose_msg.header.frame_id = "map"; + init_pose_msg.pose.pose.position.x = x1; + init_pose_msg.pose.pose.position.y = y1; + init_pose_msg.pose.pose.position.z = 0.0; + init_pose_msg.pose.pose.orientation.w = 1.0; + init_pose_msg.pose.covariance.fill(0.0); + initialpose_pub->publish(init_pose_msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update(nav_state); + + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9; + + if (ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + } +} diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index 12e237f4..8bd6781e 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/CMakeLists.txt @@ -64,7 +64,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") 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 c166f876..55a666d3 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -163,6 +163,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. * @@ -170,6 +173,13 @@ 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_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index 0371eab1..2b6dc641 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -449,7 +449,8 @@ void AMCLLocalizer::on_initialize() tf_broadcaster_ = std::make_unique(get_node()); auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); + auto rt_cbg = node_typed ? node_typed->get_real_time_cbg() : + get_node()->get_node_base_interface()->get_default_callback_group(); rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; if (!compute_odom_from_tf_) { @@ -463,6 +464,10 @@ void 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); } @@ -475,6 +480,153 @@ void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) if (!initialized_odom_) {last_odom_ = odom_; initialized_odom_ = true;} } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // Check expected frame (map) + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + last_input_time_ = msg->header.stamp; + + // 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]; + double cov_xy = cov[1]; + double var_y = cov[7]; + + // Yaw variance + double var_yaw = cov[35]; + + // 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) { + 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; + 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 (safety) + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + void AMCLLocalizer::update_odom_from_tf() { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt index b874c059..400b4523 100644 --- a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt @@ -2,8 +2,8 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_srvs REQUIRED) -ament_add_gtest(costmap_localizer_tests costmap_localizer_tests.cpp) -target_link_libraries(costmap_localizer_tests +ament_add_gtest(navmap_localizer_tests navmap_localizer_tests.cpp) +target_link_libraries(navmap_localizer_tests ${PROJECT_NAME} rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle diff --git a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp deleted file mode 100644 index 7e9166a0..00000000 --- a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// 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 - -#include "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_navmap_common/costmap_2d.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "std_srvs/srv/trigger.hpp" - -#include -#include - -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test -{ -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) -{ - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) -} - -TEST_F(AMCLLocalizerTest, 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()); - - 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)); - - auto static_map = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); -} - -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { -public: - void force_path(const std::string & path) {map_path_ = path;} -}; - -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) -{ - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); - - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - - 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); - - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); - - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); - - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); -} diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp new file mode 100644 index 00000000..89dfed8a --- /dev/null +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp @@ -0,0 +1,182 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 + +#include +#include +#include +#include + +#include "easynav_navmap_localizer/AMCLLocalizer.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ + +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; +} + +double yaw_from_tf(const tf2::Transform & tf) +{ + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; +} + +class FriendAMCLLocalizer : public easynav::navmap::AMCLLocalizer +{ +public: + using easynav::navmap::AMCLLocalizer::init_pose_sub_; +}; + +class AMCLLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = -3.0; + const double y0 = 0.25; + const double yaw0 = 0.2; + + const double x1 = 0.8; + const double y1 = 3.3; + const double yaw1 = 2.0; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.compute_odom_from_tf", true), + }); + + auto node = std::make_shared( + "test_navmap_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } +} diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index 5b61ef63..a26bb848 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/CMakeLists.txt @@ -58,7 +58,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") 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 9ba5ebab..caf6fdd7 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -155,6 +155,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. * @@ -162,6 +165,13 @@ 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); + /// List of particles representing the belief distribution. std::vector particles_; 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 42165823..d391cb4b 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -243,7 +243,8 @@ AMCLLocalizer::on_initialize() tf_broadcaster_ = std::make_unique(get_node()); auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); + auto rt_cbg = node_typed ? node_typed->get_real_time_cbg() : + get_node()->get_node_base_interface()->get_default_callback_group(); rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; @@ -256,6 +257,10 @@ AMCLLocalizer::on_initialize() estimate_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, + std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); + last_reseed_ = get_node()->now(); last_input_time_ = get_node()->now(); @@ -314,6 +319,154 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) } } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // Check expected frame (map) + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + last_input_time_ = msg->header.stamp; + + // 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]; + double cov_xy = cov[1]; + double var_y = cov[7]; + + // Yaw variance + double var_yaw = cov[35]; + + // 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) { + 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; + 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 (safety) + 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::predict([[maybe_unused]] NavState & nav_state) { diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index d353bbea..36efd008 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -15,173 +15,167 @@ #include -#include "easynav_simple_common/SimpleMap.hpp" +#include +#include +#include +#include + #include "easynav_simple_localizer/AMCLLocalizer.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test +namespace { -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - void TearDown() override - { - rclcpp::shutdown(); - } -}; - - -/// \brief Dynamic map update tests -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) { - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared(); - static_map->initialize(30, 30, 0.1, -1.5, -1.5, 0.0); - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - - perception->data.points.resize(6); - 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->data.points[2].x = -10.0; - perception->data.points[2].y = -1.0; - perception->data.points[2].z = 0.2; - perception->data.points[3].x = 10.0; - perception->data.points[3].y = -1.0; - perception->data.points[3].z = 0.2; - perception->data.points[4].x = 1.0; - perception->data.points[4].y = -10.0; - perception->data.points[4].z = 0.2; - perception->data.points[5].x = 1.0; - perception->data.points[5].y = 10.0; - perception->data.points[5].z = 0.2; - - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast(manager->get_dynamyc_map()); - ASSERT_TRUE(map_ptr != nullptr); - - auto cell1 = map_ptr->metric_to_cell(1.0, 1.0); - EXPECT_TRUE(map_ptr->at(cell1.first, cell1.second)); - - auto cell2 = map_ptr->metric_to_cell(-1.0, -1.0); - EXPECT_TRUE(map_ptr->at(cell2.first, cell2.second)); + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; } -/// \brief Map loading via subscription test -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) +double yaw_from_tf(const tf2::Transform & tf) { - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - easynav::TFInfo tf_info; - tf_info.map_frame = "world_map"; - tf_info.odom_frame = "world_odom"; - tf_info.robot_frame = "world_base"; - tf_info.robot_footprint_frame = "world_footprint_base"; - easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - rclcpp::Publisher::SharedPtr pub = - node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = tf_info.map_frame; - 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; // Mark cell (5,5) as occupied - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - - EXPECT_EQ(static_map->at(5, 5), 1); - EXPECT_EQ(static_map->at(1, 1), 0); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; } -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { +class FriendAMCLLocalizer : public easynav::AMCLLocalizer +{ public: - void force_path(const std::string & path) {map_path_ = path;} + using easynav::AMCLLocalizer::init_pose_sub_; }; -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) +class AMCLLocalizerInitialPoseTest : public ::testing::Test { - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared(); - static_map->initialize(4, 4, 0.5, -1.0, -1.0, 0); - static_map->at(1, 1) = true; - static_map->at(2, 2) = true; - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map.txt"; - const std::string service_name = "/test_savemap_node/test_savemap/savemap"; +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; - std::static_pointer_cast(manager)->force_path(test_map_file); +} // namespace - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 0.3; + const double y0 = 1.7; + const double yaw0 = -0.8; + + const double x1 = 2.2; + const double y1 = -0.4; + const double yaw1 = 1.1; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + }); + + auto node = std::make_shared( + "test_simple_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); - auto client = node->create_client(service_name); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); - std::ifstream infile(test_map_file); - ASSERT_TRUE(infile.is_open()); + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } - std::string first_line; - std::getline(infile, first_line); - EXPECT_NE(first_line.find("4 4"), std::string::npos); - EXPECT_NE(first_line.find("0.5"), std::string::npos); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } - infile.close(); - std::remove(test_map_file.c_str()); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } } 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 75691590..76751621 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 @@ -149,9 +149,6 @@ SimpleMapsManager::update(NavState & nav_state) return; } - std::cout << "perceptions size: " << perceptions.size() << std::endl; - std::cout << "perception[0] size: " << perceptions[0]->data.points.size() << std::endl; - std::cout << "perception[0] valid: " << perceptions[0]->valid << std::endl; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); rclcpp::Time stamp; From ece07a73bbb0cc0e12ab18227047ffb03ba890bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Apr 2026 07:29:45 +0200 Subject: [PATCH 178/230] Update README.md for all localizers MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- localizers/easynav_fusion_localizer/README.md | 18 +++++++++++++----- localizers/easynav_gps_localizer/README.md | 9 ++++++++- localizers/easynav_navmap_localizer/README.md | 1 + localizers/easynav_simple_localizer/README.md | 1 + 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index e48542e7..c641ef72 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -1,4 +1,4 @@ -# easynav_gps_localizer +# easynav_fusion_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)](#) @@ -23,15 +23,23 @@ Odometry fusion localizer based on Robot Localization that fuses any n odometry ## Parameters -TODO +All parameters are declared under the plugin namespace, i.e., `//easynav_fusion_localizer/FusionLocalizer/...`. -See [robot_localization documentation](https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html) +| Name | Type | Default | Description | +|---|---|---:|---| +| `.initial_pose.x` | `double` | `0.0` | Initial X position (m). | +| `.initial_pose.y` | `double` | `0.0` | Initial Y position (m). | +| `.initial_pose.yaw` | `double` | `0.0` | Initial yaw (rad). | + +For additional robot_localization-related configuration, see the upstream 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 | |---|---|---|---|---| +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Seed/reset the filter pose from an external initial pose estimate. | depth=10 | | Publisher | `odometry/filtered` | `nav_msgs/msg/Odometry` | Odometry fused from all sources. | SensorDataQoS | @@ -41,13 +49,13 @@ 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. | +| `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Fused odometry estimate. | ## TF Frames | Role | Transform | Notes | |---|---|---| -| Publishes | `map -> odom` | Static or slowly varying transform aligning UTM to local odometry frame. | +| Publishes | `map -> odom` | Transform that aligns the odometry frame with the global frame. | ## License diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index 5306d1fb..97640b1f 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -28,7 +28,13 @@ GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom- ## Parameters -This plugin does not declare configurable ROS parameters. +All parameters are declared under the plugin namespace, i.e., `//easynav_gps_localizer/GpsLocalizer/...`. + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.initial_pose.x` | `double` | `0.0` | Initial X position (m). | +| `.initial_pose.y` | `double` | `0.0` | Initial Y position (m). | +| `.initial_pose.yaw` | `double` | `0.0` | Initial yaw (rad). | ## Interfaces (Topics and Services) @@ -38,6 +44,7 @@ This plugin does not declare configurable ROS parameters. |---|---|---|---|---| | 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) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Seed/reset the localizer pose from an external initial pose estimate. | depth=10 | | Publisher | `robot/odom_gps` | `nav_msgs/msg/Odometry` | Odometry fused from GPS + IMU (UTM-projected). | SensorDataQoS | ### Services diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index b984b01b..827a6c1c 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -59,6 +59,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 | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Initialize 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` | Publishes the estimated pose with covariance | depth=10 | diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index bee15a05..8a06b1be 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -52,6 +52,7 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `/odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false (not present here). | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Initialize 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 | From fe3e2b621c1ee2862f4f5b66fb10f04d80f08e33 Mon Sep 17 00:00:00 2001 From: midemig Date: Thu, 23 Apr 2026 18:53:14 +0200 Subject: [PATCH 179/230] fix: add gnss group to gps config and ge_group has group to meet new easynav features. Initial pose now changes initial utm pose to change robot position in the map --- .../example_params_summit_dual_gazebo.yaml | 1 + .../FusionLocalizer.cpp | 60 +++++++------------ 2 files changed, 23 insertions(+), 38 deletions(-) diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml index b7afad57..7ec21016 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml @@ -267,6 +267,7 @@ sensors_node: gps: topic: /gps/fix type: sensor_msgs/msg/NavSatFix + group: gnss laser1: topic: /front_laser/points type: sensor_msgs/msg/PointCloud2 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 4f2fcdbb..a02905af 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -108,39 +108,6 @@ void FusionLocalizer::on_initialize() navsatfix_topic_, rclcpp::QoS(10)); } - // Optional initial pose from parameters (kept consistent with AMCL parameter names) - localizer_node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); - localizer_node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); - localizer_node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); - - double init_x = 0.0; - double init_y = 0.0; - double init_yaw = 0.0; - localizer_node->get_parameter(plugin_name + ".initial_pose.x", init_x); - localizer_node->get_parameter(plugin_name + ".initial_pose.y", init_y); - localizer_node->get_parameter(plugin_name + ".initial_pose.yaw", init_yaw); - - if (std::abs(init_x) > 1e-12 || std::abs(init_y) > 1e-12 || std::abs(init_yaw) > 1e-12) { - auto init_pose = std::make_shared(); - init_pose->header.stamp = localizer_node->now(); - init_pose->header.frame_id = tf_info.map_frame; - init_pose->pose.pose.position.x = init_x; - init_pose->pose.pose.position.y = init_y; - init_pose->pose.pose.position.z = 0.0; - tf2::Quaternion q; - q.setRPY(0.0, 0.0, init_yaw); - init_pose->pose.pose.orientation = tf2::toMsg(q); - init_pose->pose.covariance.fill(0.0); - - if (ukf_global_) { - ukf_global_->setPoseCallback(init_pose); - first_pose_received_ = true; - } - if (ukf_local_) { - ukf_local_->setPoseCallback(init_pose); - } - } - } catch (const std::exception & e) { RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", @@ -164,12 +131,29 @@ void FusionLocalizer::on_initialize() void FusionLocalizer::init_pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - // Forward initial pose to any active filters - if (ukf_global_) { + if (has_global_filter_) { + nav_msgs::msg::Odometry global_odom; + // Get current position to calculate the offset + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + double current_x = global_odom.pose.pose.position.x; + double current_y = global_odom.pose.pose.position.y; + + // Shift the UTM origin so that the current GPS position will now map to msg's position + UTM_origin_x_ += (current_x - msg->pose.pose.position.x); + UTM_origin_y_ += (current_y - msg->pose.pose.position.y); + + RCLCPP_INFO(get_node()->get_logger(), + "Initial pose reset UTM origin. Shifted X by %.2f, Y by %.2f to match pose (%.2f, %.2f)", + (current_x - msg->pose.pose.position.x), (current_y - msg->pose.pose.position.y), + msg->pose.pose.position.x, msg->pose.pose.position.y); + } + + // Forward initial pose to global filter to make the state jump immediately ukf_global_->setPoseCallback(msg); first_pose_received_ = true; } - if (ukf_local_) { + + if (has_local_filter_) { ukf_local_->setPoseCallback(msg); } } @@ -179,8 +163,8 @@ void FusionLocalizer::update_rt(NavState & nav_state) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (has_global_filter_) { - if (n_gps_sensors_ && nav_state.has("gnss")) { - auto gps_data = nav_state.get(std::string("gnss")); + if (n_gps_sensors_ && nav_state.has_group("gnss")) { + auto gps_data = nav_state.get_group(std::string("gnss")); const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); for (int i = 0; i < n_gps_sensors_; ++i) { if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { From ebf7405155a1fd6b13dadb54807d78f6dc63d39c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 06:38:55 +0100 Subject: [PATCH 180/230] Adding timestamp to costmap MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- common/easynav_costmap_common/CMakeLists.txt | 3 + .../easynav_costmap_common/costmap_2d.hpp | 16 +++- common/easynav_costmap_common/package.xml | 1 + .../src/easynav_costmap_common/costmap_2d.cpp | 20 ++++- .../tests/costmap_2d_tests.cpp | 74 +++++++++++++++++++ 5 files changed, 109 insertions(+), 5 deletions(-) diff --git a/common/easynav_costmap_common/CMakeLists.txt b/common/easynav_costmap_common/CMakeLists.txt index fd5578d7..243de87a 100644 --- a/common/easynav_costmap_common/CMakeLists.txt +++ b/common/easynav_costmap_common/CMakeLists.txt @@ -12,6 +12,7 @@ set(CMAKE_CXX_EXTENSIONS OFF) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -24,6 +25,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common ${nav_msgs_TARGETS} + rclcpp::rclcpp ) install( @@ -56,5 +58,6 @@ ament_export_dependencies( easynav_common # pluginlib nav_msgs + rclcpp ) ament_package() diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp index 45394e9c..fa57947b 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 @@ -44,6 +44,7 @@ #include #include #include +#include "rclcpp/time.hpp" #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -126,10 +127,19 @@ class Costmap2D * * The resulting OccupancyGrid message contains metadata such as resolution, size, and origin, * and its data field is populated with cost values from the costmap. Cells with a value of - * NO_INFORMATION are mapped to -1; other values are cast to int8_t directly. + * NO_INFORMATION are mapped to -1; other values are cast to int8_t directly. + * + * The output message header stamp is set to the internal last-modified timestamp. */ void toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const; + /** + * @brief Get timestamp of last costmap modification. + * + * Currently only set by constructors and copy/assignment; other updates do not change it. + */ + rclcpp::Time getLastModifiedStamp() const; + /** * @brief Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap @param source Source costmap where the window will be copied from @@ -580,6 +590,10 @@ class Costmap2D unsigned char * costmap_; unsigned char default_value_; + // Timestamp indicating when the costmap was last modified. + // Currently only set by constructors and copy/assignment; other updates do not change it. + rclcpp::Time last_modified_; + // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels class MarkCell { diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 7f3c271f..d9267fa6 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -21,6 +21,7 @@ easynav_common nav_msgs + rclcpp ament_index_cpp ament_lint_auto 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 91fd5928..53235782 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 @@ -51,7 +51,7 @@ Costmap2D::Costmap2D( unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value) : resolution_(resolution), origin_x_(origin_x), - origin_y_(origin_y), costmap_(NULL), default_value_(default_value) + origin_y_(origin_y), costmap_(NULL), default_value_(default_value), last_modified_(0) { access_ = new mutex_t(); @@ -61,7 +61,7 @@ Costmap2D::Costmap2D( } Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map) -: default_value_(FREE_SPACE) +: default_value_(FREE_SPACE), last_modified_(map.header.stamp) { access_ = new mutex_t(); @@ -210,6 +210,10 @@ Costmap2D::toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const { std::lock_guard lock(*access_); + const int64_t stamp_ns = last_modified_.nanoseconds(); + msg.header.stamp.sec = static_cast(stamp_ns / 1000000000LL); + msg.header.stamp.nanosec = static_cast(stamp_ns % 1000000000LL); + msg.info.width = size_x_; msg.info.height = size_y_; msg.info.resolution = resolution_; @@ -231,6 +235,12 @@ Costmap2D::toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const } } +rclcpp::Time Costmap2D::getLastModifiedStamp() const +{ + std::lock_guard lock(*access_); + return last_modified_; +} + Costmap2D & Costmap2D::operator=(const Costmap2D & map) { // check for self assignment @@ -247,6 +257,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map) origin_x_ = map.origin_x_; origin_y_ = map.origin_y_; default_value_ = map.default_value_; + last_modified_ = map.last_modified_; // initialize our various maps initMaps(size_x_, size_y_); @@ -258,7 +269,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map) } Costmap2D::Costmap2D(const Costmap2D & map) -: costmap_(NULL) +: costmap_(NULL), last_modified_(0) { access_ = new mutex_t(); *this = map; @@ -266,7 +277,8 @@ Costmap2D::Costmap2D(const Costmap2D & map) // just initialize everything to NULL by default Costmap2D::Costmap2D() -: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL) +: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), + costmap_(NULL), last_modified_(0) { access_ = new mutex_t(); } diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index a9ff17b8..49889c81 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -16,6 +16,7 @@ #include #include "easynav_costmap_common/costmap_2d.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/time.hpp" using easynav::Costmap2D; @@ -121,3 +122,76 @@ TEST_F(Costmap2DTest, OccupancyGridConversion) EXPECT_EQ(grid.data[i], expected ? 100 : 0); } } + +TEST_F(Costmap2DTest, TimestampFromOccupancyGridConstructor) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 123; + in.header.stamp.nanosec = 456u; + + in.info.width = 2u; + in.info.height = 3u; + in.info.resolution = 0.5; + in.info.origin.position.x = 1.0; + in.info.origin.position.y = -2.0; + in.data.assign(in.info.width * in.info.height, 0); + + Costmap2D map(in); + const int64_t expected_ns = 123LL * 1000000000LL + 456LL; + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns); + + nav_msgs::msg::OccupancyGrid out; + map.toOccupancyGridMsg(out); + + EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); + EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); +} + +TEST_F(Costmap2DTest, TimestampFromCopyConstructor) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 10; + in.header.stamp.nanosec = 20u; + in.info.width = 1u; + in.info.height = 1u; + in.info.resolution = 1.0; + in.info.origin.position.x = 0.0; + in.info.origin.position.y = 0.0; + in.data.assign(1u, 0); + + Costmap2D original(in); + Costmap2D copy(original); + + const int64_t expected_ns = 10LL * 1000000000LL + 20LL; + EXPECT_EQ(copy.getLastModifiedStamp().nanoseconds(), expected_ns); + + nav_msgs::msg::OccupancyGrid out; + copy.toOccupancyGridMsg(out); + EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); + EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); +} + +TEST_F(Costmap2DTest, TimestampNotUpdatedByChanges) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 7; + in.header.stamp.nanosec = 8u; + in.info.width = 3u; + in.info.height = 3u; + in.info.resolution = 1.0; + in.info.origin.position.x = 0.0; + in.info.origin.position.y = 0.0; + in.data.assign(in.info.width * in.info.height, 0); + + Costmap2D map(in); + const int64_t expected_ns = 7LL * 1000000000LL + 8LL; + map.setCost(1, 1, 100); + map.resizeMap(4u, 4u, 1.0, 1.0, 1.0); + + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns); + + nav_msgs::msg::OccupancyGrid out; + map.toOccupancyGridMsg(out); + EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); + EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); +} From d60661c058f4f0ff2bf821585a615aa997d7545c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 06:56:05 +0100 Subject: [PATCH 181/230] Change map static to base 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 | 2 +- .../AMCLLocalizer.cpp | 6 +-- .../tests/costmap_localizer_tests.cpp | 2 +- .../easynav_costmap_maps_manager/README.md | 10 ++-- .../CostmapMapsManager.hpp | 14 +++--- .../filters/InflationFilter.hpp | 12 ++--- .../CostmapMapsManager.cpp | 36 +++++++-------- .../filters/InflationFilter.cpp | 46 +++++++++---------- .../tests/costmap_mapsmanager_tests.cpp | 24 +++++----- 9 files changed, 76 insertions(+), 76 deletions(-) diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index c1590195..998ee961 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -66,7 +66,7 @@ This package does not create service servers or clients. | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. | -| `map.static` | `Costmap2D` | **Read** | Static costmap for likelihood evaluation. | +| `map.base` | `Costmap2D` | **Read** | Base costmap for likelihood evaluation. | ## TF Frames 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 7c26a2c0..d229c8cb 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -572,12 +572,12 @@ AMCLLocalizer::correct(NavState & nav_state) return; } - if (!nav_state.has("map.static")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); + if (!nav_state.has("map.base")) { + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map"); return; } - const auto & map_static = nav_state.get("map.static"); + const auto & map_static = nav_state.get("map.base"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index bc03cbe6..8f7bd0ac 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -67,7 +67,7 @@ TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) navstate.set("perceptions", easynav::Perceptions()); navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); + navstate.set("map.base", *static_map); manager->update(navstate); diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index ebdf66c9..3de9cd38 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -2,7 +2,7 @@ ## 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. +Maps Manager that maintains 2D costmaps (base 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]: @@ -81,7 +81,7 @@ Each entry in `.filters` defines a sub-namespace `.` wit - **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. + The filter reads both the base 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:** @@ -95,7 +95,7 @@ Each entry in `.filters` defines a sub-namespace `.` wit | Key | Type | Access | Description | |---|---|---|---| -| `map.static` | `Costmap2D` | **Read** | Static costmap to inflate. | +| `map.base` | `Costmap2D` | **Read** | Base 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. | @@ -130,7 +130,7 @@ maps_manager_node: | 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` | -| Publisher | `//map` | `nav_msgs/msg/OccupancyGrid` | Publishes the static costmap. | `depth=1` | +| Publisher | `//map` | `nav_msgs/msg/OccupancyGrid` | Publishes the base costmap. | `depth=1` | | Publisher | `//dynamic_map` | `nav_msgs/msg/OccupancyGrid` | Publishes the dynamic (live) costmap. | `depth=100` | ### Services @@ -145,7 +145,7 @@ maps_manager_node: | Key | Type | Access | Notes | |---|---|---|---| -| `map.static` | `Costmap2D` | **Write** | Static map loaded from YAML. | +| `map.base` | `Costmap2D` | **Write** | Base 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_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 b36ee8ff..cfef0727 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 @@ -78,7 +78,7 @@ class CostmapMapsManager : public easynav::MapsManagerBase * * @param new_map Shared pointer to a new map object. Must be of type SimpleMap. */ - void set_static_map(const Costmap2D & new_map); + void set_base_map(const Costmap2D & new_map); protected: /** @@ -88,9 +88,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase private: /** - * @brief Internal static map. + * @brief Internal base map. */ - Costmap2D static_map_; + Costmap2D map_base_; /** * @brief Internal static map. @@ -98,9 +98,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase std::shared_ptr dynamic_map_; /** - * @brief Publisher for the static occupancy grid. + * @brief Publisher for the base occupancy grid. */ - rclcpp::Publisher::SharedPtr static_occ_pub_; + rclcpp::Publisher::SharedPtr base_occ_pub_; /** * @brief Publisher for the dynamic occupancy grid. @@ -118,9 +118,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase rclcpp::Service::SharedPtr savemap_srv_; /** - * @brief Cached occupancy grid message for the static map. + * @brief Cached occupancy grid message for the base map. */ - nav_msgs::msg::OccupancyGrid static_grid_msg_; + nav_msgs::msg::OccupancyGrid base_grid_msg_; /** * @brief Cached occupancy grid message for the dynamic map. 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 20e9a086..4c2cc037 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,20 +230,20 @@ class InflationFilter : public CostmapFilter bool matchedSize_ {false}; - Costmap2D static_inflated_; - bool has_static_inflated_ {false}; + Costmap2D base_inflated_; + bool has_base_inflated_ {false}; - struct StaticGeomSignature + struct BaseGeomSignature { 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_; + } base_sig_; - bool needs_recompute_static_(const Costmap2D & static_map) const; - void recompute_static_inflation_(const Costmap2D & static_map); + bool needs_recompute_base_(const Costmap2D & base_map) const; + void recompute_base_inflation_(const Costmap2D & base_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 ea4e4f14..79fb44b7 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 @@ -98,7 +98,7 @@ CostmapMapsManager::on_initialize() } } - static_occ_pub_ = node->create_publisher( + base_occ_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); @@ -116,30 +116,30 @@ CostmapMapsManager::on_initialize() throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } - if (auto ret = loadMapFromYaml(map_path_, static_grid_msg_) != LOAD_MAP_SUCCESS) { + if (auto ret = loadMapFromYaml(map_path_, base_grid_msg_) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } - static_map_ = Costmap2D(static_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = node->now(); - static_grid_msg_.header.frame_id = tf_info.map_frame; - static_grid_msg_.header.stamp = node->now(); - static_occ_pub_->publish(static_grid_msg_); + map_base_ = Costmap2D(base_grid_msg_); + base_occ_pub_->publish(base_grid_msg_); } incoming_map_sub_ = node->create_subscription( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map", rclcpp::QoS(1).transient_local().reliable(), [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { - static_grid_msg_ = *msg; + base_grid_msg_ = *msg; - static_map_ = Costmap2D(*msg); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = this->get_node()->now(); - static_grid_msg_.header.frame_id = tf_info.map_frame; - static_grid_msg_.header.stamp = this->get_node()->now(); + map_base_ = Costmap2D(base_grid_msg_); - static_occ_pub_->publish(static_grid_msg_); + base_occ_pub_->publish(base_grid_msg_); }); savemap_srv_ = node->create_service( @@ -157,7 +157,7 @@ CostmapMapsManager::on_initialize() params.free_thresh = 0.25; params.occupied_thresh = 0.65; - if (!saveMapToFile(static_grid_msg_, params)) { + if (!saveMapToFile(base_grid_msg_, params)) { response->success = false; response->message = "Failed to save map to: " + map_path_; } else { @@ -168,9 +168,9 @@ CostmapMapsManager::on_initialize() } void -CostmapMapsManager::set_static_map(const Costmap2D & new_map) +CostmapMapsManager::set_base_map(const Costmap2D & new_map) { - static_map_ = new_map; + map_base_ = new_map; } @@ -179,14 +179,14 @@ CostmapMapsManager::update(NavState & nav_state) { EASYNAV_TRACE_EVENT; - if (!nav_state.has("map.static")) { - nav_state.set("map.static", static_map_); + if (!nav_state.has("map.base")) { + nav_state.set("map.base", map_base_); } if (!dynamic_map_) { - dynamic_map_ = std::make_shared(static_map_); + dynamic_map_ = std::make_shared(map_base_); } else { - *dynamic_map_ = static_map_; + *dynamic_map_ = map_base_; } nav_state.set("map.dynamic.filtered", dynamic_map_); 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 4aeffd9a..6df5384b 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 @@ -102,10 +102,10 @@ InflationFilter::update(NavState & nav_state) 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"); + const auto & base_map = nav_state.get("map.base"); - if (needs_recompute_static_(static_map)) { - recompute_static_inflation_(static_map); + if (needs_recompute_base_(base_map)) { + recompute_base_inflation_(base_map); } if (!matchedSize_) { @@ -149,7 +149,7 @@ InflationFilter::update(NavState & nav_state) 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.getCost(i, j), base_inflated_.getCost(i, j)); dynamic_map.setCost(i, j, cost); } } @@ -443,17 +443,17 @@ InflationFilter::generateIntegerDistances() } bool -InflationFilter::needs_recompute_static_(const Costmap2D & static_map) const +InflationFilter::needs_recompute_base_(const Costmap2D & base_map) const { - if (!has_static_inflated_) { + if (!has_base_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) + if (base_map.getSizeInCellsX() != base_sig_.size_x || + base_map.getSizeInCellsY() != base_sig_.size_y || + base_map.getResolution() != base_sig_.resolution || + base_map.getOriginX() != base_sig_.origin_x || + base_map.getOriginY() != base_sig_.origin_y) { return true; } @@ -462,22 +462,22 @@ InflationFilter::needs_recompute_static_(const Costmap2D & static_map) const } void -InflationFilter::recompute_static_inflation_(const Costmap2D & static_map) +InflationFilter::recompute_base_inflation_(const Costmap2D & base_map) { - static_inflated_ = static_map; + base_inflated_ = base_map; - matchSize(static_inflated_); - const int w = static_inflated_.getSizeInCellsX(); - const int h = static_inflated_.getSizeInCellsY(); - updateCosts(static_inflated_, 0, 0, w, h); + matchSize(base_inflated_); + const int w = base_inflated_.getSizeInCellsX(); + const int h = base_inflated_.getSizeInCellsY(); + updateCosts(base_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(); + base_sig_.size_x = base_map.getSizeInCellsX(); + base_sig_.size_y = base_map.getSizeInCellsY(); + base_sig_.resolution = base_map.getResolution(); + base_sig_.origin_x = base_map.getOriginX(); + base_sig_.origin_y = base_map.getOriginY(); - has_static_inflated_ = true; + has_base_inflated_ = true; } } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index 8440f0c2..2852fd45 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 @@ -43,8 +43,8 @@ class CostmapMapsManagerTest : public ::testing::Test // auto manager = std::make_shared(); // manager->initialize(node, "test"); // -// easynav::Costmap2D static_map(30, 30, 0.1, -1.5, -1.5); -// manager->set_static_map(static_map); +// easynav::Costmap2D base_map(30, 30, 0.1, -1.5, -1.5); +// manager->set_base_map(base_map); // // easynav::NavState navstate; // auto perception = std::make_shared(); @@ -108,8 +108,8 @@ class CostmapMapsManagerTest : public ::testing::Test // easynav::NavState navstate; // manager->update(navstate); // -// ASSERT_TRUE(navstate.has("map.static")); -// const auto & map = navstate.get("map.static"); +// ASSERT_TRUE(navstate.has("map.base")); +// const auto & map = navstate.get("map.base"); // // EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); // EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); @@ -129,17 +129,17 @@ class CostmapMapsManagerTest : public ::testing::Test // auto manager = std::make_shared(); // manager->initialize(node, "test_savemap"); // -// // Create a 200x200 static costmap with resolution 0.05 and origin at (0, 0) +// // Create a 200x200 base costmap with resolution 0.05 and origin at (0, 0) // const unsigned int width = 200; // const unsigned int height = 200; -// easynav::Costmap2D map_static(width, height, 0.05, 0.0, 0.0); +// easynav::Costmap2D map_base(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); +// map_base.setCost(30, y, easynav::LETHAL_OBSTACLE); // } // -// manager->set_static_map(map_static); +// manager->set_base_map(map_base); // // const std::string yaml_path = "/tmp/savemap_test_map"; // const std::string service_name = "/test_savemap_node/test_savemap/savemap"; @@ -169,11 +169,11 @@ class CostmapMapsManagerTest : public ::testing::Test // easynav::Costmap2D loaded_map(loaded_grid); // // // Check map dimensions match -// ASSERT_EQ(loaded_map.getSizeInCellsX(), map_static.getSizeInCellsX()); -// ASSERT_EQ(loaded_map.getSizeInCellsY(), map_static.getSizeInCellsY()); +// ASSERT_EQ(loaded_map.getSizeInCellsX(), map_base.getSizeInCellsX()); +// ASSERT_EQ(loaded_map.getSizeInCellsY(), map_base.getSizeInCellsY()); // -// for (unsigned int y = 0; y < map_static.getSizeInCellsY(); ++y) { -// for (unsigned int x = 0; x < map_static.getSizeInCellsX(); ++x) { +// for (unsigned int y = 0; y < map_base.getSizeInCellsY(); ++y) { +// for (unsigned int x = 0; x < map_base.getSizeInCellsX(); ++x) { // if (x == 30) { // EXPECT_EQ(loaded_map.getCost(x, y), easynav::LETHAL_OBSTACLE) // << "Expected LETHAL_OBSTACLE at (" << x << "," << y << ")"; From 6ac95ae109eba9030fb416866fe05ba22a081481 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 07:10:23 +0100 Subject: [PATCH 182/230] Costamp base map sincronized with nav_state MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CostmapMapsManager.cpp | 15 +- .../tests/costmap_mapsmanager_tests.cpp | 161 +++++++++++++++++- 2 files changed, 174 insertions(+), 2 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 79fb44b7..bc57f90b 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 @@ -179,7 +179,20 @@ CostmapMapsManager::update(NavState & nav_state) { EASYNAV_TRACE_EVENT; - if (!nav_state.has("map.base")) { + // Sync internal base map with NavState, preferring the newest stamp. + // Note: Compare using nanoseconds to avoid rclcpp::Time clock-type mismatches. + if (nav_state.has("map.base")) { + const auto & external_base = nav_state.get("map.base"); + + const int64_t internal_ns = map_base_.getLastModifiedStamp().nanoseconds(); + const int64_t external_ns = external_base.getLastModifiedStamp().nanoseconds(); + + if (external_ns > internal_ns) { + map_base_ = external_base; + } else if (internal_ns > external_ns) { + nav_state.set("map.base", map_base_); + } + } else { nav_state.set("map.base", map_base_); } 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 2852fd45..19cf79db 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 @@ -15,12 +15,22 @@ #include -#include "easynav_sensors/types/PointPerception.hpp" +#include +#include +#include + #include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_costmap_common/costmap_2d.hpp" +#include "easynav_costmap_common/cost_values.hpp" +#include "easynav_costmap_maps_manager/CostmapMapsManager.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + /// \brief Fixture for CostmapMapsManager tests class CostmapMapsManagerTest : public ::testing::Test { @@ -36,6 +46,155 @@ class CostmapMapsManagerTest : public ::testing::Test } }; +static nav_msgs::msg::OccupancyGrid make_grid( + int width, + int height, + int occ_x, + int occ_y, + int64_t stamp_ns) +{ + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = "map"; + grid.header.stamp.sec = static_cast(stamp_ns / 1000000000LL); + grid.header.stamp.nanosec = static_cast(stamp_ns % 1000000000LL); + + grid.info.width = static_cast(width); + grid.info.height = static_cast(height); + grid.info.resolution = 0.2f; + grid.info.origin.position.x = -1.0; + grid.info.origin.position.y = -1.0; + grid.data.assign(static_cast(width * height), 0); + + if (occ_x >= 0 && occ_y >= 0 && occ_x < width && occ_y < height) { + grid.data[static_cast(occ_y * width + occ_x)] = 100; + } + + return grid; +} + +TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerNavStateMap) +{ + auto node = std::make_shared("test_node_sync_ns"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + const int64_t older_ns = 10LL * 1000000000LL; + const int64_t newer_ns = 20LL * 1000000000LL; + + easynav::Costmap2D internal_old(make_grid(10, 10, 1, 1, older_ns)); + easynav::Costmap2D external_new(make_grid(10, 10, 2, 2, newer_ns)); + + manager->set_base_map(internal_old); + + easynav::NavState navstate; + navstate.set("map.base", external_new); + + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); + + ASSERT_TRUE(navstate.has("map.dynamic.filtered")); + const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); + EXPECT_EQ(dyn_ptr->getCost(2, 2), easynav::LETHAL_OBSTACLE); + EXPECT_EQ(dyn_ptr->getCost(1, 1), easynav::FREE_SPACE); +} + +TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerInternalMap) +{ + auto node = std::make_shared("test_node_sync_internal"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + const int64_t older_ns = 10LL * 1000000000LL; + const int64_t newer_ns = 20LL * 1000000000LL; + + easynav::Costmap2D internal_new(make_grid(10, 10, 3, 3, newer_ns)); + easynav::Costmap2D external_old(make_grid(10, 10, 4, 4, older_ns)); + + manager->set_base_map(internal_new); + + easynav::NavState navstate; + navstate.set("map.base", external_old); + + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); + + ASSERT_TRUE(navstate.has("map.dynamic.filtered")); + const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); + EXPECT_EQ(dyn_ptr->getCost(3, 3), easynav::LETHAL_OBSTACLE); + EXPECT_EQ(dyn_ptr->getCost(4, 4), easynav::FREE_SPACE); +} + +TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) +{ + auto node = std::make_shared("test_node_incoming"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + + const std::string topic = node->get_fully_qualified_name() + std::string("/test/incoming_map"); + auto pub = node->create_publisher( + topic, rclcpp::QoS(1).transient_local().reliable()); + pub->on_activate(); + + auto grid = make_grid(10, 10, 5, 5, 123LL * 1000000000LL); + pub->publish(grid); + + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor.spin_some(); + + easynav::NavState navstate; + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getCost(5, 5), easynav::LETHAL_OBSTACLE); + EXPECT_GT(base_after.getLastModifiedStamp().nanoseconds(), 0); + + ASSERT_TRUE(navstate.has("map.dynamic.filtered")); + const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getCost(5, 5), easynav::LETHAL_OBSTACLE); + EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), + base_after.getLastModifiedStamp().nanoseconds()); +} + ///// \brief Dynamic map update test with point cloud //TEST_F(CostmapMapsManagerTest, BasicDynamicUpdate) //{ From 91c7e1b719beceac4451a1f3deafe9f571763fa1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 08:25:06 +0100 Subject: [PATCH 183/230] Publish changes in base map and be ready to save updated map MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CostmapMapsManager.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) 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 bc57f90b..66be2125 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 @@ -171,6 +171,14 @@ void CostmapMapsManager::set_base_map(const Costmap2D & new_map) { map_base_ = new_map; + + if (base_occ_pub_) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + map_base_.toOccupancyGridMsg(base_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = map_base_.getLastModifiedStamp(); + base_occ_pub_->publish(base_grid_msg_); + } } @@ -189,6 +197,14 @@ CostmapMapsManager::update(NavState & nav_state) if (external_ns > internal_ns) { map_base_ = external_base; + + if (base_occ_pub_) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + map_base_.toOccupancyGridMsg(base_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = map_base_.getLastModifiedStamp(); + base_occ_pub_->publish(base_grid_msg_); + } } else if (internal_ns > external_ns) { nav_state.set("map.base", map_base_); } From 7ff4876e49c362e0acd2d1421187b5254ebfa052 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 08:39:03 +0100 Subject: [PATCH 184/230] Any change in the costmap set it as modified MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_common/costmap_2d.hpp | 14 ++++++- .../src/easynav_costmap_common/costmap_2d.cpp | 37 ++++++++++++++++++- .../tests/costmap_2d_tests.cpp | 16 +++++--- .../filters/InflationFilter.hpp | 2 + .../filters/InflationFilter.cpp | 6 ++- 5 files changed, 66 insertions(+), 9 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 fa57947b..e0750d4a 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 @@ -136,10 +136,20 @@ class Costmap2D /** * @brief Get timestamp of last costmap modification. * - * Currently only set by constructors and copy/assignment; other updates do not change it. + * Set by constructors and copy/assignment, and may be updated by some mutating operations. + * For high-frequency per-cell updates (e.g., many `setCost()` calls in a loop), prefer calling + * `touch()` once after the batch update. */ rclcpp::Time getLastModifiedStamp() const; + /** + * @brief Mark the costmap as modified. + * + * This bumps the internal last-modified stamp by 1 nanosecond while preserving the clock type. + * It is intended to be called after bulk updates to avoid per-cell overhead. + */ + void touch(); + /** * @brief Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap @param source Source costmap where the window will be copied from @@ -591,7 +601,7 @@ class Costmap2D unsigned char default_value_; // Timestamp indicating when the costmap was last modified. - // Currently only set by constructors and copy/assignment; other updates do not change it. + // Set by constructors and copy/assignment, and bumped by some mutating operations. rclcpp::Time last_modified_; // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels 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 53235782..71b19648 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 @@ -126,6 +126,9 @@ void Costmap2D::resetMaps() { std::unique_lock lock(*access_); memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char)); + + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn) @@ -141,6 +144,9 @@ void Costmap2D::resetMapToValue( for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_) { memset(costmap_ + y, value, len * sizeof(unsigned char)); } + + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } bool Costmap2D::copyCostmapWindow( @@ -179,6 +185,11 @@ bool Costmap2D::copyCostmapWindow( map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_); + + { + std::unique_lock lock(*access_); + last_modified_ = map.getLastModifiedStamp(); + } return true; } @@ -202,6 +213,14 @@ bool Costmap2D::copyWindow( source.costmap_, sx0, sy0, source.size_x_, costmap_, dx0, dy0, size_x_, sz_x, sz_y); + + { + const auto source_stamp = source.getLastModifiedStamp(); + std::unique_lock lock(*access_); + const int64_t new_stamp_ns = std::max(last_modified_.nanoseconds(), + source_stamp.nanoseconds()) + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); + } return true; } @@ -241,6 +260,13 @@ rclcpp::Time Costmap2D::getLastModifiedStamp() const return last_modified_; } +void Costmap2D::touch() +{ + std::lock_guard lock(*access_); + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); +} + Costmap2D & Costmap2D::operator=(const Costmap2D & map) { // check for self assignment @@ -317,7 +343,16 @@ unsigned char Costmap2D::getCost(unsigned int undex) const void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) { - costmap_[getIndex(mx, my)] = cost; + std::lock_guard lock(*access_); + const unsigned int index = getIndex(mx, my); + + if (costmap_[index] == cost) { + return; + } + + costmap_[index] = cost; + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index 49889c81..bfef3f94 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -171,7 +171,7 @@ TEST_F(Costmap2DTest, TimestampFromCopyConstructor) EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); } -TEST_F(Costmap2DTest, TimestampNotUpdatedByChanges) +TEST_F(Costmap2DTest, TimestampUpdatedBySetCost) { nav_msgs::msg::OccupancyGrid in; in.header.stamp.sec = 7; @@ -185,13 +185,19 @@ TEST_F(Costmap2DTest, TimestampNotUpdatedByChanges) Costmap2D map(in); const int64_t expected_ns = 7LL * 1000000000LL + 8LL; + map.setCost(1, 1, 100); - map.resizeMap(4u, 4u, 1.0, 1.0, 1.0); + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns + 1); - EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns); + // Setting the same value should not change the stamp. + map.setCost(1, 1, 100); + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns + 1); nav_msgs::msg::OccupancyGrid out; map.toOccupancyGridMsg(out); - EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); - EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); + + const int64_t out_ns = + static_cast(out.header.stamp.sec) * 1000000000LL + + static_cast(out.header.stamp.nanosec); + EXPECT_EQ(out_ns, expected_ns + 1); } 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 4c2cc037..9a6f9b32 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,6 +42,7 @@ #ifndef EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ +#include #include #include @@ -240,6 +241,7 @@ class InflationFilter : public CostmapFilter double resolution {0.0}; double origin_x {0.0}; double origin_y {0.0}; + int64_t stamp_ns {0}; } base_sig_; bool needs_recompute_base_(const Costmap2D & base_map) const; 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 6df5384b..4df61c69 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 @@ -449,11 +449,14 @@ InflationFilter::needs_recompute_base_(const Costmap2D & base_map) const return true; } + const int64_t stamp_ns = base_map.getLastModifiedStamp().nanoseconds(); + if (base_map.getSizeInCellsX() != base_sig_.size_x || base_map.getSizeInCellsY() != base_sig_.size_y || base_map.getResolution() != base_sig_.resolution || base_map.getOriginX() != base_sig_.origin_x || - base_map.getOriginY() != base_sig_.origin_y) + base_map.getOriginY() != base_sig_.origin_y || + stamp_ns != base_sig_.stamp_ns) { return true; } @@ -476,6 +479,7 @@ InflationFilter::recompute_base_inflation_(const Costmap2D & base_map) base_sig_.resolution = base_map.getResolution(); base_sig_.origin_x = base_map.getOriginX(); base_sig_.origin_y = base_map.getOriginY(); + base_sig_.stamp_ns = base_map.getLastModifiedStamp().nanoseconds(); has_base_inflated_ = true; } From 2b526fdc633c04fcc550f69866ac1a467ead0198 Mon Sep 17 00:00:00 2001 From: midemig Date: Fri, 24 Apr 2026 12:57:37 +0200 Subject: [PATCH 185/230] deleted tests related to initial pose param which was removed. Removed spaces --- .../FusionLocalizer.cpp | 10 ++-- .../tests/fusion_localizer_tests.cpp | 46 ++++--------------- 2 files changed, 15 insertions(+), 41 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 a02905af..8488f319 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -137,22 +137,22 @@ void FusionLocalizer::init_pose_callback( if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { double current_x = global_odom.pose.pose.position.x; double current_y = global_odom.pose.pose.position.y; - + // Shift the UTM origin so that the current GPS position will now map to msg's position UTM_origin_x_ += (current_x - msg->pose.pose.position.x); UTM_origin_y_ += (current_y - msg->pose.pose.position.y); - - RCLCPP_INFO(get_node()->get_logger(), + + RCLCPP_INFO(get_node()->get_logger(), "Initial pose reset UTM origin. Shifted X by %.2f, Y by %.2f to match pose (%.2f, %.2f)", (current_x - msg->pose.pose.position.x), (current_y - msg->pose.pose.position.y), msg->pose.pose.position.x, msg->pose.pose.position.y); } - + // Forward initial pose to global filter to make the state jump immediately ukf_global_->setPoseCallback(msg); first_pose_received_ = true; } - + if (has_local_filter_) { ukf_local_->setPoseCallback(msg); } diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp index 1cbac580..d010b352 100644 --- a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp @@ -73,9 +73,6 @@ TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbac rclcpp::NodeOptions options; options.parameter_overrides({ rclcpp::Parameter("test.global_filter.frequency", 30.0), - rclcpp::Parameter("test.initial_pose.x", x0), - rclcpp::Parameter("test.initial_pose.y", y0), - rclcpp::Parameter("test.initial_pose.yaw", yaw0), }); auto node = std::make_shared(options); @@ -116,32 +113,6 @@ TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbac } easynav::NavState nav_state; - { - const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); - while (std::chrono::steady_clock::now() < deadline) { - exec.spin_some(); - localizer->update_rt(nav_state); - if (nav_state.has("robot_pose")) { - const auto odom = nav_state.get("robot_pose"); - const bool ok = - std::abs(odom.pose.pose.position.x - x0) < 1e-6 && - std::abs(odom.pose.pose.position.y - y0) < 1e-6 && - std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw0) < 1e-3; - if (ok) { - break; - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } - - ASSERT_TRUE(nav_state.has("robot_pose")); - { - const auto odom = nav_state.get("robot_pose"); - EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); - EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); - EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-3); - } geometry_msgs::msg::PoseWithCovarianceStamped init_pose_msg; init_pose_msg.header.stamp = pub_node->now(); @@ -161,19 +132,22 @@ TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbac exec.spin_some(); localizer->update_rt(nav_state); - const auto odom = nav_state.get("robot_pose"); - const bool ok = - std::abs(odom.pose.pose.position.x - x1) < 1e-6 && - std::abs(odom.pose.pose.position.y - y1) < 1e-6 && - std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-3; + if (nav_state.has("robot_pose")) { + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x1) < 1e-6 && + std::abs(odom.pose.pose.position.y - y1) < 1e-6 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-3; - if (ok) { - break; + if (ok) { + break; + } } std::this_thread::sleep_for(std::chrono::milliseconds(10)); } } + ASSERT_TRUE(nav_state.has("robot_pose")); { const auto odom = nav_state.get("robot_pose"); EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-6); From d703a824ce3307928bd400f446eaa9b1e4b2c282 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 3 May 2026 09:51:45 +0200 Subject: [PATCH 186/230] Cells cost have no real impact in path creation 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 | 9 ++++----- 1 file changed, 4 insertions(+), 5 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 f6bfdf20..702f5be8 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -140,7 +140,7 @@ void CostmapPlanner::on_initialize() void CostmapPlanner::update(NavState & nav_state) { - if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { + if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map")) { return; } @@ -150,7 +150,7 @@ void CostmapPlanner::update(NavState & nav_state) return; } - const auto & map = nav_state.get("map.dynamic"); + const auto & map = nav_state.get("map"); 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(); @@ -264,7 +264,6 @@ 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 = 1.0; const double diagonal_cost = std::sqrt(2.0); auto idx = [&](int x, int y) {return y * width + x;}; @@ -296,8 +295,8 @@ std::vector CostmapPlanner::a_star_path( // 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_norm; + // Calculate traversal cost: cost_factor_ acts as a direct multiplier on cell cost + double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost); 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; From 997294507bc51f39138468001db1e4245be3296d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 3 May 2026 09:53:30 +0200 Subject: [PATCH 187/230] Navstate key among filters are always map, not an arbitrary key MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../SerestController.cpp | 2 +- localizers/easynav_simple_localizer/README.md | 2 +- .../easynav_simple_localizer/AMCLLocalizer.cpp | 6 +++--- .../easynav_costmap_maps_manager/README.md | 14 +++++++------- .../CostmapMapsManager.cpp | 4 ++-- .../filters/InflationFilter.cpp | 8 ++++---- .../filters/ObstacleFilter.cpp | 4 ++-- .../tests/costmap_mapsmanager_tests.cpp | 16 ++++++++-------- .../tests/navmap_mapsmanager_tests.cpp | 8 ++++---- .../easynav_routes_maps_manager/README.md | 4 ++-- .../filters/RoutesCostmapFilter.hpp | 4 ++-- .../filters/RoutesCostmapFilter.cpp | 4 ++-- .../tests/routes_costmap_filter_tests.cpp | 10 +++++----- .../easynav_simple_maps_manager/README.md | 4 ++-- .../SimpleMapsManager.cpp | 8 ++++---- .../tests/simple_mapsmanager_tests.cpp | 8 ++++---- planners/easynav_costmap_planner/README.md | 2 +- planners/easynav_simple_planner/README.md | 2 +- .../src/easynav_simple_planner/SimplePlanner.cpp | 10 +++++----- 19 files changed, 60 insertions(+), 60 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 9a32f1ff..52272e5f 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -375,7 +375,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")) { + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map")) { publish_stop(nav_state, tf_info.robot_footprint_frame); return false; } diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 8a06b1be..428f59cf 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -65,7 +65,7 @@ This package does not create service servers or clients. | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. | -| `map.static` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | +| `map.base` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Estimated robot pose. | ## TF Frames 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 d391cb4b..723eb5cf 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -527,12 +527,12 @@ void AMCLLocalizer::correct(NavState & nav_state) return; } - if (!nav_state.has("map.static")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); + if (!nav_state.has("map.base")) { + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map"); return; } - const auto & map_static = nav_state.get("map.static"); + const auto & map_static = nav_state.get("map.base"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 3de9cd38..67e6c9a0 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -72,8 +72,8 @@ Each entry in `.filters` defines a sub-namespace `.` wit | 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. | +| `map` | `Costmap2D` | **Write** | Marks cells as `LETHAL_OBSTACLE` (254). | +| `map.obstacle_bounds` | `ObstacleBounds` | **Write** | Bounding box of updated obstacles for incremental inflation. | #### **InflationFilter** @@ -96,8 +96,8 @@ Each entry in `.filters` defines a sub-namespace `.` wit | Key | Type | Access | Description | |---|---|---|---| | `map.base` | `Costmap2D` | **Read** | Base 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. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic costmap input and output after inflation. | +| `map.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. @@ -146,9 +146,9 @@ maps_manager_node: | Key | Type | Access | Notes | |---|---|---|---| | `map.base` | `Costmap2D` | **Write** | Base 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). | +| `map` | `Costmap2D` | **Write** | Dynamic map after applying filters. | +| `map` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | +| `map.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | --- 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 66be2125..0a4bb5bc 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 @@ -218,7 +218,7 @@ CostmapMapsManager::update(NavState & nav_state) *dynamic_map_ = map_base_; } - nav_state.set("map.dynamic.filtered", dynamic_map_); + nav_state.set("map", dynamic_map_); if (!nav_state.has("map_time")) { nav_state.set("map_time", get_node()->now()); @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map", dynamic_map_); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 4df61c69..f03c72d1 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,7 +99,7 @@ InflationFilter::on_initialize() void InflationFilter::update(NavState & nav_state) { - auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map"); Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & base_map = nav_state.get("map.base"); @@ -121,8 +121,8 @@ InflationFilter::update(NavState & nav_state) int max_i = size_x; int max_j = size_y; - if (nav_state.has("map.dynamic.obstacle_bounds")) { - const auto & bb = nav_state.get("map.dynamic.obstacle_bounds"); + if (nav_state.has("map.obstacle_bounds")) { + const auto & bb = nav_state.get("map.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) && @@ -154,7 +154,7 @@ InflationFilter::update(NavState & nav_state) } } - nav_state.set("map.dynamic.filtered", dynamic_map_ptr); + nav_state.set("map", dynamic_map_ptr); } 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 69e41672..d29fab15 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 @@ -47,7 +47,7 @@ ObstacleFilter::update(NavState & nav_state) return; } - auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map"); Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -81,7 +81,7 @@ ObstacleFilter::update(NavState & nav_state) if (!fused.empty()) { ObstacleBounds bb{min_x, min_y, max_x, max_y}; - nav_state.set("map.dynamic.obstacle_bounds", bb); + nav_state.set("map.obstacle_bounds", bb); } } 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 19cf79db..d962bfd4 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 @@ -103,8 +103,8 @@ TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerNavStateMap) const auto & base_after = navstate.get("map.base"); EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); EXPECT_EQ(dyn_ptr->getCost(2, 2), easynav::LETHAL_OBSTACLE); @@ -142,8 +142,8 @@ TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerInternalMap) const auto & base_after = navstate.get("map.base"); EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); EXPECT_EQ(dyn_ptr->getCost(3, 3), easynav::LETHAL_OBSTACLE); @@ -187,8 +187,8 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) EXPECT_EQ(base_after.getCost(5, 5), easynav::LETHAL_OBSTACLE); EXPECT_GT(base_after.getLastModifiedStamp().nanoseconds(), 0); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getCost(5, 5), easynav::LETHAL_OBSTACLE); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), @@ -224,8 +224,8 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) // // manager->update(navstate); // -// ASSERT_TRUE(navstate.has("map.dynamic")); -// const auto & map = navstate.get("map.dynamic"); +// ASSERT_TRUE(navstate.has("map")); +// const auto & map = navstate.get("map"); // // unsigned int cx, cy; // ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); 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 4c55b8fe..913234a8 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 @@ -68,8 +68,8 @@ class NavMapMapsManagerTest : public ::testing::Test // // manager->update(::easynav::NavState); // -// ASSERT_TRUE(::easynav::NavState.has("map.dynamic")); -// const auto & map = ::easynav::NavState.get("map.dynamic"); +// ASSERT_TRUE(::easynav::NavState.has("map")); +// const auto & map = ::easynav::NavState.get("map"); // // unsigned int cx, cy; // ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); @@ -111,8 +111,8 @@ class NavMapMapsManagerTest : public ::testing::Test // ::easynav::NavState ::easynav::NavState; // manager->update(::easynav::NavState); // -// ASSERT_TRUE(::easynav::NavState.has("map.static")); -// const auto & map = ::easynav::NavState.get("map.static"); +// ASSERT_TRUE(::easynav::NavState.has("map.base")); +// const auto & map = ::easynav::NavState.get("map.base"); // // EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); // EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md index 1bf0af58..3e1ba4f2 100644 --- a/maps_managers/easynav_routes_maps_manager/README.md +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -97,7 +97,7 @@ has been written to the NavState on each update cycle. - **Plugin Name:** `easynav_routes_maps_manager/RoutesCostmapFilter` - **Type:** `easynav::RoutesCostmapFilter` - **Description:** - Reads the current `RoutesMap` and a dynamic costmap (`map.dynamic.filtered`) + Reads the current `RoutesMap` and a dynamic costmap (`map`) 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. @@ -114,7 +114,7 @@ has been written to the NavState on each update cycle. | 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`. | +| `map` | `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 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 7ea65635..975883ba 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 @@ -28,7 +28,7 @@ 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 +/// ("map") 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. @@ -56,7 +56,7 @@ class RoutesCostmapFilter : public RoutesFilter /// @brief Apply the routes-based filtering to the costmap. /// - /// If both "routes" and "map.dynamic.filtered" entries are present + /// If both "routes" and "map" 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. 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 10a2a897..e5fb7b2d 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 @@ -73,13 +73,13 @@ RoutesCostmapFilter::update(NavState & nav_state) return; } - if (!nav_state.has("map.dynamic.filtered")) { + if (!nav_state.has("map")) { return; } const auto & routes = nav_state.get("routes"); - auto costmap_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto costmap_ptr = nav_state.get_ptr("map"); if (!costmap_ptr) { return; } 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 a6f574ca..011e462e 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 @@ -58,7 +58,7 @@ TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); NavState nav_state; - // No 'routes' and no 'map.dynamic.filtered' keys -> update should not crash + // No 'routes' and no 'map' keys -> update should not crash EXPECT_NO_THROW(filter.update(nav_state)); } @@ -93,11 +93,11 @@ TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) NavState nav_state; nav_state.set("routes", routes); - nav_state.set("map.dynamic.filtered", map); + nav_state.set("map", map); filter.update(nav_state); - auto map_after = nav_state.get("map.dynamic.filtered"); + auto map_after = nav_state.get("map"); // Cells clearly along the interior of the route should remain 0, // others should be >= 50 (default min_cost). The last endpoint @@ -142,11 +142,11 @@ TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) NavState nav_state; nav_state.set("routes", routes); - nav_state.set("map.dynamic.filtered", map); + nav_state.set("map", map); filter.update(nav_state); - auto map_after = nav_state.get("map.dynamic.filtered"); + auto map_after = nav_state.get("map"); // No cell should be considered "on route"; all should be raised to >= 50 for (unsigned int x = 0; x < 5; ++x) { diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 10f59ab4..00755612 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -58,8 +58,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. | +| `map.base` | `SimpleMap` | **Write** | Static map loaded from file / parameter configuration. | +| `map` | `SimpleMap` | **Write** | Dynamic map after applying incoming updates. | ## TF Frames 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 76751621..a711d4cf 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 @@ -144,8 +144,8 @@ SimpleMapsManager::update(NavState & nav_state) } if (perceptions.empty()) { - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map.base", static_map_); + nav_state.set("map", dynamic_map_); return; } @@ -166,8 +166,8 @@ SimpleMapsManager::update(NavState & nav_state) } } - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map.base", static_map_); + nav_state.set("map", dynamic_map_); dynamic_map_.to_occupancy_grid(dynamic_grid_msg_); dynamic_grid_msg_.header.frame_id = tf_info.map_frame; 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 e6adb415..449dee29 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 @@ -99,8 +99,8 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) manager->update(navstate); - ASSERT_TRUE(navstate.has("map.dynamic")); - auto map = navstate.get("map.dynamic"); + ASSERT_TRUE(navstate.has("map")); + auto map = navstate.get("map"); auto cell1 = map.metric_to_cell(1.0, 1.0); EXPECT_TRUE(map.at(cell1.first, cell1.second)); @@ -148,8 +148,8 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) manager->update(navstate); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ASSERT_TRUE(navstate.has("map.static")); - const auto & map = navstate.get("map.static"); + ASSERT_TRUE(navstate.has("map.base")); + const auto & map = navstate.get("map.base"); EXPECT_EQ(map.at(5, 5), 1); EXPECT_EQ(map.at(1, 1), 0); diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index 6750fca3..df142bd3 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -96,7 +96,7 @@ This plugin does not create subscriptions or services directly; it reads inputs | Key | Type | Access | Notes | |---|---|---|---| | `goals` | `nav_msgs::msg::Goals` | **Read** | Goal list used as planner targets. | -| `map.dynamic` | `Costmap2D` | **Read** | Dynamic costmap used for A* search. | +| `map` | `Costmap2D` | **Read** | Dynamic costmap used for A* search. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose used as the start position. | | `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | diff --git a/planners/easynav_simple_planner/README.md b/planners/easynav_simple_planner/README.md index 324dc632..efe694cc 100644 --- a/planners/easynav_simple_planner/README.md +++ b/planners/easynav_simple_planner/README.md @@ -47,7 +47,7 @@ This plugin does not create subscriptions or services directly; it reads inputs | Key | Type | Access | Notes | |---|---|---|---| | `goals` | `nav_msgs::msg::Goals` | **Read** | Planner targets. | -| `map.dynamic` | `SimpleMap` | **Read** | Dynamic `SimpleMap` grid used for search. | +| `map` | `SimpleMap` | **Read** | Dynamic `SimpleMap` grid used for search. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Start pose for path planning. | | `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | 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 b869c68c..aa8f707d 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -113,16 +113,16 @@ SimplePlanner::update(NavState & nav_state) return; } - if (!nav_state.has("map.dynamic")) { - RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update map.dynamic map not found"); + if (!nav_state.has("map")) { + RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update map map not found"); return; } SimpleMap map_typed; - if (nav_state.has("map.dynamic")) { - map_typed = nav_state.get("map.dynamic"); + if (nav_state.has("map")) { + map_typed = nav_state.get("map"); } else { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.dynamic map"); + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map"); return; } From 0be0003c266e98413c2e59d08602b03fcc596a15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:24:29 +0200 Subject: [PATCH 188/230] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- maps_managers/easynav_costmap_maps_manager/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 67e6c9a0..7acf6015 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -146,8 +146,7 @@ maps_manager_node: | Key | Type | Access | Notes | |---|---|---|---| | `map.base` | `Costmap2D` | **Write** | Base map loaded from YAML. | -| `map` | `Costmap2D` | **Write** | Dynamic map after applying filters. | -| `map` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic map after applying filters; if an existing filtered `map` is available, the manager may use it as input. | | `map.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | --- From 54f155abadba845821c969ad0effaf59018620f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:24:56 +0200 Subject: [PATCH 189/230] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- .../easynav_costmap_maps_manager/filters/ObstacleFilter.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 d29fab15..dfce79fa 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 @@ -48,6 +48,10 @@ ObstacleFilter::update(NavState & nav_state) } auto dynamic_map_ptr = nav_state.get_ptr("map"); + if (!dynamic_map_ptr) { + RCLCPP_WARN(get_node()->get_logger(), "There is no map in NavState"); + return; + } Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); From b3c84262a0fb450367d4bdf4f1e82038818dbef6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:29:14 +0200 Subject: [PATCH 190/230] Avoid rewrite map with dynamic_map 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 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0a4bb5bc..6e3231f3 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 @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - nav_state.set("map", dynamic_map_); + dynamic_map_ = nav_state.get>("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); From e9428a3dce1a087a08eb146492bf288c5329cf5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 5 May 2026 07:40:33 +0200 Subject: [PATCH 191/230] Update dynamic_map_ with map key at navstate 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 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6e3231f3..f763f0a5 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 @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - dynamic_map_ = nav_state.get>("map"); + *dynamic_map_ = nav_state.get("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); From 6fb5634f450926b847e5e3af4d730be72545471d Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Mon, 6 Jul 2026 11:57:17 +0200 Subject: [PATCH 192/230] Fix rolling CI --- .github/workflows/rolling.yaml | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index d70f2b65..cafefba5 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -12,18 +12,16 @@ on: workflow_dispatch: jobs: build-and-test: - runs-on: ubuntu-24.04 + runs-on: ubuntu-26.04 container: - image: ubuntu:noble + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-rolling-ros-base:master steps: - name: Repo checkout - uses: actions/checkout@v6-beta + uses: actions/checkout@v6 with: ref: rolling - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 + uses: ros-tooling/action-ros-ci@0.4.8 with: package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: rolling From b63922baa5455a5aad11345576b0d6693e8014b2 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Mon, 6 Jul 2026 12:23:19 +0200 Subject: [PATCH 193/230] Update calls to deprecated get_package_share_directory --- .../src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp | 6 +++--- .../src/easynav_costmap_maps_manager/CostmapMapsManager.cpp | 4 ++-- .../src/easynav_navmap_maps_manager/NavMapMapsManager.cpp | 6 +++--- .../src/easynav_octomap_maps_manager/OctomapMapsManager.cpp | 6 +++--- .../src/easynav_routes_maps_manager/RoutesMapsManager.cpp | 6 +++--- .../src/easynav_simple_maps_manager/SimpleMapsManager.cpp | 4 ++-- 6 files changed, 16 insertions(+), 16 deletions(-) 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 b5222471..4ad4cfb4 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 @@ -27,7 +27,7 @@ #include "pcl/point_types.h" #include "pcl/point_cloud.h" -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav_bonxai @@ -73,7 +73,7 @@ BonxaiMapsManager::on_initialize() if (!package_name.empty() && !bonxai_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + bonxai_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); @@ -106,7 +106,7 @@ BonxaiMapsManager::on_initialize() if (!package_name.empty() && !occmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 66be2125..3f8507e4 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 @@ -24,7 +24,7 @@ #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_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav @@ -110,7 +110,7 @@ CostmapMapsManager::on_initialize() map_path_ = "/tmp/default.map.yaml"; if (!package_name.empty() && !map_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + map_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 aa0d04e4..2e9bdf48 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 @@ -26,7 +26,7 @@ #include "navmap_ros/navmap_io.hpp" #include "easynav_navmap_maps_manager/map_io.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav @@ -122,7 +122,7 @@ NavMapMapsManager::on_initialize() if (!package_name.empty() && !occmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); @@ -145,7 +145,7 @@ NavMapMapsManager::on_initialize() if (!package_name.empty() && !navmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + navmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 e8e1a6c6..c79fd50a 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 @@ -29,7 +29,7 @@ #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_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav @@ -113,7 +113,7 @@ OctomapMapsManager::on_initialize() // if (!package_name.empty() && !occmap_path_file.empty()) { // try { -// const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); +// const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { // throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); @@ -136,7 +136,7 @@ OctomapMapsManager::on_initialize() // // if (!package_name.empty() && !octomap_path_file.empty()) { // try { -// const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); +// const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { // throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 3c0645f3..d12884bf 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,7 @@ #include -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_share_path.hpp" #include "rclcpp/rclcpp.hpp" @@ -84,8 +84,8 @@ void RoutesMapsManager::on_initialize() // 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; + const auto pkgpath = ament_index_cpp::get_package_share_path(package_name); + map_path_ = pkgpath / map_path_file; } else { throw std::runtime_error( "Parameters '" + plugin_name + ".package' and '" + plugin_name + 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 76751621..2681b6ea 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 @@ -19,7 +19,7 @@ #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "easynav_sensors/types/PointPerception.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" #include "easynav_common/YTSession.hpp" @@ -62,7 +62,7 @@ SimpleMapsManager::on_initialize() if (package_name != "" && map_path_file != "") { std::string pkgpath; try { - pkgpath = ament_index_cpp::get_package_share_directory(package_name); + pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + "/" + map_path_file; } catch(ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); From 80fa490159affa60d671ccf2cbfe64a02638435b Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Tue, 7 Jul 2026 22:29:09 +0200 Subject: [PATCH 194/230] Check if std is non-positive before creating std::normal_distribution --- .../AMCLLocalizer.cpp | 14 +++++++++++++ .../tests/costmap_localizer_tests.cpp | 20 +++++++++---------- .../AMCLLocalizer.cpp | 16 +++++++++++++++ .../tests/navmap_localizer_tests.cpp | 20 +++++++++---------- .../AMCLLocalizer.cpp | 15 ++++++++++++++ .../tests/simple_localizer_tests.cpp | 20 +++++++++---------- 6 files changed, 75 insertions(+), 30 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 d229c8cb..033cc8b6 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -224,6 +224,20 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); reseed_time_ = 1.0 / reseed_freq; + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles); RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index 6259a598..0ce41b2e 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -79,10 +79,10 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), rclcpp::Parameter("test.initial_pose.yaw", yaw0), - rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), - rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), - rclcpp::Parameter("test.min_noise_xy", 0.0), - rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), rclcpp::Parameter("test.compute_odom_from_tf", true), }); @@ -95,16 +95,16 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG { const tf2::Transform tf = localizer->getEstimatedPose(); - EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); - EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); - EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); } { const nav_msgs::msg::Odometry odom = localizer->get_pose(); - EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); - EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); - EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); } const auto infos = node->get_subscriptions_info_by_topic("initialpose"); 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 2b6dc641..fe3b726f 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -415,6 +415,22 @@ void AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".top_keep_fraction", top_keep_fraction_); node->get_parameter(plugin_name + ".downsampled_cloud_size", downsampled_cloud_size_); + + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + + if (!std::isfinite(inflation_stddev_) || inflation_stddev_ <= 0.0) {inflation_stddev_ = 1.5;} if (!std::isfinite(inflation_prob_min_) || inflation_prob_min_ <= 0.0) { inflation_prob_min_ = 0.01; diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp index 89dfed8a..720c89f7 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp @@ -77,10 +77,10 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), rclcpp::Parameter("test.initial_pose.yaw", yaw0), - rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), - rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), - rclcpp::Parameter("test.min_noise_xy", 0.0), - rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), rclcpp::Parameter("test.compute_odom_from_tf", true), }); @@ -94,16 +94,16 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG { const tf2::Transform tf = localizer->getEstimatedPose(); - EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); - EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); - EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); } { const nav_msgs::msg::Odometry odom = localizer->get_pose(); - EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); - EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); - EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); } const auto infos = node->get_subscriptions_info_by_topic("initialpose"); 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 d391cb4b..3f1dbb1e 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -216,6 +216,21 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + + double reseed_freq; node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); reseed_time_ = 1.0 / reseed_freq; diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index 36efd008..12dd1636 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -77,10 +77,10 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), rclcpp::Parameter("test.initial_pose.yaw", yaw0), - rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), - rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), - rclcpp::Parameter("test.min_noise_xy", 0.0), - rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), }); auto node = std::make_shared( @@ -93,16 +93,16 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG { const tf2::Transform tf = localizer->getEstimatedPose(); - EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); - EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); - EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); } { const nav_msgs::msg::Odometry odom = localizer->get_pose(); - EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); - EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); - EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); } const auto infos = node->get_subscriptions_info_by_topic("initialpose"); From f77dce8ef2216ac174f6c59c3533e659ed9c9314 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 16 Jul 2026 09:55:01 +0200 Subject: [PATCH 195/230] Port of Regulated Pure Pursuit. Initial commit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CMakeLists.txt | 83 +++ .../easynav_regulated_pp_controller/README.md | 168 ++++++ ...asynav_regulated_pp_controller_plugins.xml | 9 + .../RegulatedPurePursuitController.hpp | 196 +++++++ .../dynamic_window_pure_pursuit_functions.hpp | 328 +++++++++++ .../regulation_functions.hpp | 147 +++++ .../package.xml | 32 ++ .../RegulatedPurePursuitController.cpp | 531 ++++++++++++++++++ .../tests/CMakeLists.txt | 4 + .../tests/regulated_pp_controller_tests.cpp | 257 +++++++++ 10 files changed, 1755 insertions(+) create mode 100644 controllers/easynav_regulated_pp_controller/CMakeLists.txt create mode 100644 controllers/easynav_regulated_pp_controller/README.md create mode 100644 controllers/easynav_regulated_pp_controller/easynav_regulated_pp_controller_plugins.xml create mode 100644 controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp create mode 100644 controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp create mode 100644 controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp create mode 100644 controllers/easynav_regulated_pp_controller/package.xml create mode 100644 controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp create mode 100644 controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt create mode 100644 controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp diff --git a/controllers/easynav_regulated_pp_controller/CMakeLists.txt b/controllers/easynav_regulated_pp_controller/CMakeLists.txt new file mode 100644 index 00000000..c8757528 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_regulated_pp_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_system REQUIRED) +find_package(pluginlib REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) + + +add_library(${PROJECT_NAME} SHARED + src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + easynav_system::easynav_system + tf2_ros::tf2_ros + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + pluginlib::pluginlib + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_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 controller plugin +pluginlib_export_plugin_description_file(easynav_core easynav_regulated_pp_controller_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + easynav_system + pluginlib + tf2_ros + tf2 + tf2_geometry_msgs + geometry_msgs + nav_msgs + std_msgs +) +ament_package() diff --git a/controllers/easynav_regulated_pp_controller/README.md b/controllers/easynav_regulated_pp_controller/README.md new file mode 100644 index 00000000..7522dd45 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/README.md @@ -0,0 +1,168 @@ +# easynav_regulated_pp_controller + +Port of the [Nav2 Regulated Pure Pursuit Controller](https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html) +to the EasyNav plugin architecture (`easynav::ControllerMethodBase`), including its optional +Dynamic Window Pure Pursuit (DWPP) extension. + +- S. Macenski, S. Singh, F. Martin, J. Gines, [**Regulated Pure Pursuit for Robot Path Tracking**](https://arxiv.org/abs/2305.20026). Autonomous Robots, 2023. +- Fumiya Ohnishi and Masaki Takahashi, **DWPP: Dynamic Window Pure Pursuit Considering Velocity and Acceleration Constraints**, arXiv:2601.15006, 2026. + +```bibtex +@article{macenski2023regulated, + title={Regulated Pure Pursuit for Robot Path Tracking}, + author={Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines}, + year={2023}, + journal = {Autonomous Robots} +} +``` + +## Algorithm + +The controller finds a lookahead ("carrot") point on the path at a given distance from the +robot and drives towards it, following the classic Pure Pursuit geometry: the curvature needed +to reach the carrot point is `k = 2y / (x^2+y^2)` in the robot's local frame. On top of that, it +adds the *regulation* terms from the paper above: + +- **Velocity-scaled lookahead distance** (Adaptive Pure Pursuit): the lookahead distance grows + with speed, clamped between `min_lookahead_dist` and `max_lookahead_dist`. +- **Curvature regulation**: linear velocity is reduced on sharp turns (small turning radius). +- **Obstacle-proximity regulation**: linear velocity is reduced close to obstacles. +- **Approach-to-goal regulation**: linear velocity is reduced smoothly as the robot nears the + end of the path. +- **Rotate to rough heading / rotate to goal heading**: the robot rotates in place first if the + carrot (or the final goal) is at a large angle from the current heading. +- **Dynamic Window Pure Pursuit (optional)**: instead of directly applying `angular_vel = linear_vel * curvature`, + it searches the reachable (velocity, angular velocity) space under the configured + acceleration/deceleration limits for the point that best matches the desired curvature. + +## Adaptations from the Nav2 version + +EasyNav's design differs from Nav2's `controller_server` in ways that require some adaptations: + +- **No costmap.** EasyNav controllers only have access to the robot pose, the path, and fused + point-cloud perceptions (`PointPerception`) via the `NavState` blackboard — there is no + costmap or inflation layer to query. The cost-based linear velocity regulation term + (`use_cost_regulated_linear_velocity_scaling` in Nav2) is therefore replaced by an equivalent + term, `use_obstacle_regulated_linear_velocity_scaling`, driven by the distance to the nearest + point in the fused perception cloud within the lookahead corridor, rather than by inflated + costmap cost (see `heuristics::obstacleConstraint` in + [`regulation_functions.hpp`](include/easynav_regulated_pp_controller/regulation_functions.hpp)). +- **No per-controller collision-arc checking.** Nav2's RPP throws a `NoValidControl` exception + when it predicts a collision along a forward-simulated arc. EasyNav already performs this kind + of safety stop uniformly for every controller in `ControllerMethodBase` (see the shared + `colision_checker.*` parameters), so this controller does not duplicate it. +- **No separate goal-checker plugin.** Nav2 relies on an independent `GoalChecker` plugin to + decide when the robot has reached the goal. This controller checks the distance/angle to the + last path pose directly, using `goal_tolerance.position` / `goal_tolerance.yaw` from the + `NavState` if `GoalManager` has published them, or the `xy_goal_tolerance` / + `yaw_goal_tolerance` parameters otherwise — consistent with how other EasyNav controllers + (e.g. `easynav_simple_controller`, `easynav_mpc_controller`) handle it. +- **No TF-based path transform.** EasyNav controllers assume `path` and `robot_pose` are already + expressed in the same frame (see `easynav_simple_controller`, `easynav_mpc_controller`), so + there is no `transformPathInTargetFrame` step. The path-to-robot-frame conversion needed by the + Pure Pursuit geometry (curvature is only meaningful with the robot at the local origin facing + +x) is done analytically from the robot pose instead of via `tf2`. +- **No plan pruning upstream.** Nav2's `controller_server` prunes the global plan to a local + window starting at (or near) the robot before handing it to the controller. EasyNav's `path` in + `NavState` is the full, un-pruned plan, so the carrot search (`getLookAheadPoint`) explicitly + starts from the path pose closest to the robot (`findClosestPoseIndex`) rather than from index 0 + of the array. Skipping this step is a real trap, not just a style choice: once the robot has + travelled more than a lookahead distance away from the path's start pose, that start pose itself + would satisfy "farther than the lookahead distance", so a naive search from index 0 keeps + returning it as the carrot for the rest of the path — freezing the carrot behind the robot, + producing bogus large curvature/angle values, and making the controller rotate-in-place far more + than it should. +- **No `setSpeedLimit` API.** EasyNav has no equivalent to Nav2's dynamic speed-limiting + interface, so it is not ported. + +## Parameters + +| Parameter | Description | +|-----|----| +| `max_linear_vel` | Maximum linear velocity. | +| `min_linear_vel` | Minimum linear velocity, used when `use_dynamic_window` is `true`. | +| `max_angular_vel` / `min_angular_vel` | Angular velocity bounds, used when `use_dynamic_window` is `true`. | +| `max_linear_accel` / `max_linear_decel` | Linear acceleration/deceleration bounds, used when `use_dynamic_window` is `true`. | +| `max_angular_accel` / `max_angular_decel` | Angular acceleration/deceleration bounds; also used by `rotate_to_heading`. | +| `lookahead_dist` | Fixed lookahead distance to find the carrot point. | +| `min_lookahead_dist` / `max_lookahead_dist` | Bounds for the velocity-scaled lookahead distance. | +| `lookahead_time` | Gain used to scale the lookahead distance by the current speed. | +| `use_velocity_scaled_lookahead_dist` | Use velocity-scaled lookahead distance instead of the fixed `lookahead_dist`. | +| `rotate_to_heading_angular_vel` | Angular velocity used while rotating in place. | +| `use_rotate_to_heading` | Enable rotate-in-place behaviors (rough path heading and final goal heading). | +| `rotate_to_heading_min_angle` | Angle to the carrot beyond which the robot rotates in place first. | +| `use_regulated_linear_velocity_scaling` | Enable curvature-based velocity regulation. | +| `regulated_linear_scaling_min_radius` | Turning radius below which curvature regulation kicks in. | +| `regulated_linear_scaling_min_speed` | Minimum velocity kept under regulation. | +| `use_fixed_curvature_lookahead` | Use a separate, fixed lookahead distance to compute the regulation curvature. | +| `curvature_lookahead_dist` | Distance of the fixed curvature lookahead, if enabled. | +| `interpolate_curvature_after_goal` | Extrapolate the curvature carrot past the goal to avoid end-of-path oscillation. | +| `use_obstacle_regulated_linear_velocity_scaling` | Enable obstacle-proximity velocity regulation (EasyNav adaptation of Nav2's cost-based term). | +| `obstacle_scaling_dist` | Distance below which obstacle regulation is triggered. | +| `obstacle_scaling_gain` | Gain (`<= 1.0`) applied when scaling down the velocity near obstacles. | +| `min_approach_linear_velocity` | Minimum velocity while approaching the goal. | +| `approach_velocity_scaling_dist` | Remaining path distance at which approach-to-goal slow-down starts. | +| `allow_reversing` | Allow driving backwards when the carrot point is behind the robot. | +| `use_dynamic_window` | Enable the Dynamic Window Pure Pursuit (DWPP) extension. | +| `xy_goal_tolerance` / `yaw_goal_tolerance` | Fallback goal tolerances used if `GoalManager` has not published `goal_tolerance.*` in the `NavState`. | + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `lookahead_point` | `geometry_msgs/PointStamped` | The current carrot point on the path. | +| `curvature_lookahead_point` | `geometry_msgs/PointStamped` | The fixed curvature lookahead point, when `use_fixed_curvature_lookahead` is enabled. | +| `is_rotating_to_heading` | `std_msgs/Bool` | Whether the controller is currently rotating in place. | + +## Example configuration + +```yaml +controller: + ros__parameters: + controller_plugin: "easynav_regulated_pp_controller/RegulatedPurePursuitController" + RegulatedPurePursuitController: + max_linear_vel: 0.5 + min_linear_vel: -0.5 + max_angular_vel: 2.5 + min_angular_vel: -2.5 + max_linear_accel: 2.5 + max_linear_decel: 2.5 + max_angular_accel: 3.2 + max_angular_decel: 3.2 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + use_velocity_scaled_lookahead_dist: false + rotate_to_heading_angular_vel: 1.8 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + use_obstacle_regulated_linear_velocity_scaling: false + obstacle_scaling_dist: 0.3 + obstacle_scaling_gain: 1.0 + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 1.0 + allow_reversing: false + use_dynamic_window: false + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 +``` + +## Tuning notes + +Same guidance as upstream Nav2 applies: + +- For Adaptive Pure Pursuit behavior, disable all boolean regulation parameters except + `use_velocity_scaled_lookahead_dist`, and tune `lookahead_time`, `min_lookahead_dist` and + `max_lookahead_dist`. +- For plain Pure Pursuit behavior, disable all boolean parameters and tune `lookahead_dist`. +- `use_obstacle_regulated_linear_velocity_scaling` can over-trigger in tightly cluttered + environments (every point in a narrow corridor is "close"); tune `obstacle_scaling_dist` and + `obstacle_scaling_gain` for the platform's environment, the same way `cost_scaling_dist` / + `cost_scaling_gain` are tuned in Nav2. diff --git a/controllers/easynav_regulated_pp_controller/easynav_regulated_pp_controller_plugins.xml b/controllers/easynav_regulated_pp_controller/easynav_regulated_pp_controller_plugins.xml new file mode 100644 index 00000000..c5c47a8c --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/easynav_regulated_pp_controller_plugins.xml @@ -0,0 +1,9 @@ + + + + + EasyNav port of the Nav2 Regulated Pure Pursuit Controller, with optional Dynamic Window Pure Pursuit (DWPP) extension. + + + + diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp new file mode 100644 index 00000000..4c4e8d36 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -0,0 +1,196 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_REGULATED_PP_CONTROLLER__REGULATEDPUREPURSUITCONTROLLER_HPP_ +#define EASYNAV_REGULATED_PP_CONTROLLER__REGULATEDPUREPURSUITCONTROLLER_HPP_ + +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/header.hpp" + +#include "easynav_core/ControllerMethodBase.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_sensors/types/PointPerception.hpp" + +namespace easynav +{ + +/// \brief Port of nav2_regulated_pure_pursuit_controller to EasyNav. +/// +/// Implements the Regulated Pure Pursuit algorithm described in: +/// S. Macenski, S. Singh, F. Martin, J. Gines, "Regulated Pure Pursuit for Robot Path Tracking", +/// Autonomous Robots, 2023 (https://arxiv.org/abs/2305.20026), plus the optional Dynamic Window +/// Pure Pursuit (DWPP) extension. See the package README for the adaptations made to fit +/// EasyNav's design (no costmap, no separate goal-checker/TF-transform plugins). +class RegulatedPurePursuitController : public ControllerMethodBase +{ +public: + RegulatedPurePursuitController(); + + /// \brief Destructor. + ~RegulatedPurePursuitController() override; + + /// \brief Declares and reads parameters. + /// \throws std::runtime_error on initialization failure. + void on_initialize() override; + + /// \brief Computes the next velocity command from the current NavState. + /// \param nav_state Current navigation state, including odometry and planned path. + void update_rt(NavState & nav_state) override; + +protected: + // --- Lookahead --- + double lookahead_dist_{0.6}; ///< Fixed lookahead distance (m). + double min_lookahead_dist_{0.3}; ///< Minimum velocity-scaled lookahead distance (m). + double max_lookahead_dist_{0.9}; ///< Maximum velocity-scaled lookahead distance (m). + double lookahead_time_{1.5}; ///< Lookahead gain: seconds projected at current speed. + bool use_velocity_scaled_lookahead_dist_{false}; ///< Use velocity-scaled lookahead distance. + + // --- Velocity / acceleration limits --- + double max_linear_vel_{0.5}; ///< Maximum linear velocity (m/s). + double min_linear_vel_{-0.5}; ///< Minimum linear velocity, used when use_dynamic_window is true (m/s). + double max_angular_vel_{2.5}; ///< Maximum angular velocity (rad/s). + double min_angular_vel_{-2.5}; ///< Minimum angular velocity, used when use_dynamic_window is true (rad/s). + double max_linear_accel_{2.5}; ///< Maximum linear acceleration (m/s^2). + double max_linear_decel_{2.5}; ///< Maximum linear deceleration (m/s^2). + double max_angular_accel_{3.2}; ///< Maximum angular acceleration (rad/s^2). + double max_angular_decel_{3.2}; ///< Maximum angular deceleration (rad/s^2). + bool use_dynamic_window_{false}; ///< Use the Dynamic Window Pure Pursuit (DWPP) extension. + bool allow_reversing_{false}; ///< Allow driving backwards when the carrot is behind the robot. + + // --- Rotate to heading --- + bool use_rotate_to_heading_{true}; ///< Enable rotate-in-place behaviors. + double rotate_to_heading_angular_vel_{1.8}; ///< Angular velocity used while rotating in place. + double rotate_to_heading_min_angle_{0.785}; ///< Angle to path beyond which to rotate in place. + + // --- Curvature regulation --- + bool use_regulated_linear_velocity_scaling_{true}; ///< Enable curvature-based regulation. + double regulated_linear_scaling_min_radius_{0.9}; ///< Turning radius that triggers regulation. + double regulated_linear_scaling_min_speed_{0.25}; ///< Minimum speed kept under regulation. + bool use_fixed_curvature_lookahead_{false}; ///< Use a separate lookahead for curvature. + double curvature_lookahead_dist_{1.0}; ///< Distance of the fixed curvature lookahead. + bool interpolate_curvature_after_goal_{false}; ///< Extrapolate curvature carrot past the goal. + + // --- Obstacle-proximity regulation (adapts the costmap-based term to EasyNav's perception) --- + bool use_obstacle_regulated_linear_velocity_scaling_{false}; ///< Enable obstacle-proximity regulation. + double obstacle_scaling_dist_{0.3}; ///< Distance below which obstacle regulation is triggered (m). + double obstacle_scaling_gain_{1.0}; ///< Gain (<=1.0) applied when scaling down the velocity. + + // --- Approach to goal --- + double min_approach_linear_velocity_{0.05}; ///< Minimum linear velocity while approaching goal. + double approach_velocity_scaling_dist_{1.0}; ///< Remaining-path distance at which to start slowing. + + // --- Goal tolerances (fallback values if "goal_tolerance.*" is not present in NavState) --- + double xy_goal_tolerance_{0.25}; ///< Fallback positional tolerance to the goal (m). + double yaw_goal_tolerance_{0.25}; ///< Fallback angular tolerance to the goal (rad). + + rclcpp::Publisher::SharedPtr lookahead_point_pub_; + rclcpp::Publisher::SharedPtr curvature_lookahead_point_pub_; + rclcpp::Publisher::SharedPtr is_rotating_to_heading_pub_; + + rclcpp::Time last_update_ts_; ///< Timestamp of the last control update. + double last_linear_vel_{0.0}; ///< Last commanded linear velocity (open-loop feedback). + double last_angular_vel_{0.0}; ///< Last commanded angular velocity (open-loop feedback). + bool is_rotating_to_heading_{false}; ///< Whether the controller is currently rotating in place. + geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. + + /// \brief Publishes a zero-velocity command and resets the open-loop velocity feedback. + void stop(NavState & nav_state, const std_msgs::msg::Header & header); + + /// \brief Computes the lookahead distance to search for the carrot pose. + /// \param linear_vel Linear velocity used to scale the lookahead distance, if enabled. + double getLookAheadDistance(double linear_vel) const; + + /// \brief Index of the path pose closest to \p robot_position. + /// + /// Since EasyNav controllers receive the full, un-pruned global path (see the README), the + /// search for the carrot pose must start from this index instead of from the beginning of the + /// path: otherwise, once the robot has advanced more than a lookahead distance away from the + /// path's start pose, that start pose itself would (incorrectly) be picked up as "farther than + /// the lookahead distance" and returned as the carrot, even though it lies behind the robot. + static std::size_t findClosestPoseIndex( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position); + + /// \brief Finds the pose on the path at (approximately) \p lookahead_dist from \p robot_position. + /// + /// Walks the path (in the same global frame as the robot pose), starting from the pose closest + /// to the robot (see \ref findClosestPoseIndex), looking for the first pose from there onwards + /// that is farther than \p lookahead_dist from the robot, then interpolates the exact carrot + /// position on the segment leading to it via a circle/segment intersection. If the remainder of + /// the path is closer than \p lookahead_dist, the path's last pose is returned (extrapolated one + /// extra segment if \p interpolate_after_end is true and the path has at least two poses). + static geometry_msgs::msg::Point getLookAheadPoint( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position, + double lookahead_dist, + bool interpolate_after_end = false); + + /// \brief Intersection of the segment [p1, p2] with the circle of radius \p r centered at \p center. + /// \return The intersection point closer to \p p2, or \p p2 itself if there is no real intersection. + static geometry_msgs::msg::Point circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r, + const geometry_msgs::msg::Point & center); + + /// \brief Expresses a global-frame point in the robot's local frame (robot at origin, facing +x). + static geometry_msgs::msg::Point toRobotFrame( + const geometry_msgs::msg::Point & point, + const geometry_msgs::msg::Pose & robot_pose, + double robot_yaw); + + /// \brief Whether the robot should rotate in place towards \p angle_to_path. + bool shouldRotateToPath(double angle_to_path) const; + + /// \brief Computes a kinematically-feasible rotate-in-place command towards \p angle_to_target. + void rotateToHeading( + double & linear_vel, double & angular_vel, double angle_to_target, double dt) const; + + /// \brief Applies curvature and obstacle-proximity regulation, then approach-to-goal scaling. + void applyConstraints( + double curvature, + double min_obstacle_distance, + double remaining_path_distance, + double euclidean_dist_to_goal, + double & linear_vel) const; + + /// \brief Distance (m) from the robot to the nearest fused perception point in front of it, + /// within \p max_range. Returns +infinity if there is no perception data or no nearby point. + double computeMinObstacleDistance(NavState & nav_state, double max_range) const; + + /// \brief Integrated path length from the robot position to the end of \p path. + static double remainingPathDistance( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position); + + /// \brief Publishes a PointStamped marker for a carrot pose, if the publisher has subscribers. + void publishCarrot( + const rclcpp::Publisher::SharedPtr & pub, + const geometry_msgs::msg::Point & carrot, + const std_msgs::msg::Header & header) const; +}; + +} // namespace easynav + +#endif // EASYNAV_REGULATED_PP_CONTROLLER__REGULATEDPUREPURSUITCONTROLLER_HPP_ diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp new file mode 100644 index 00000000..381bc708 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp @@ -0,0 +1,328 @@ +// Copyright (c) 2025 Fumiya Ohnishi +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + +/// \file +/// \brief Dynamic Window Pure Pursuit (DWPP) helper functions, ported from +/// nav2_regulated_pure_pursuit_controller. These are plain math functions over velocities and +/// accelerations, so they carry over to EasyNav unchanged. +/// +/// Fumiya Ohnishi and Masaki Takahashi, "DWPP: Dynamic Window Pure Pursuit Considering Velocity +/// and Acceleration Constraints", arXiv:2601.15006, 2026. + +#ifndef EASYNAV_REGULATED_PP_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ +#define EASYNAV_REGULATED_PP_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" + +namespace easynav +{ + +namespace dynamic_window_pure_pursuit +{ + +struct DynamicWindowBounds +{ + double max_linear_vel; + double min_linear_vel; + double max_angular_vel; + double min_angular_vel; +}; + +/** + * @brief Compute the dynamic window (feasible velocity bounds) based on + * the current speed and the given velocity and acceleration constraints. + * @param current_speed Current linear and angular velocity of the robot + * @param max_linear_vel Maximum allowable linear velocity + * @param min_linear_vel Minimum allowable linear velocity + * @param max_angular_vel Maximum allowable angular velocity + * @param min_angular_vel Minimum allowable angular velocity + * @param max_linear_accel Maximum allowable linear acceleration + * @param max_linear_decel Maximum allowable linear deceleration + * @param max_angular_accel Maximum allowable angular acceleration + * @param max_angular_decel Maximum allowable angular deceleration + * @param dt Control duration + * @return Computed dynamic window's velocity bounds + */ +inline DynamicWindowBounds computeDynamicWindow( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & dt +) +{ + DynamicWindowBounds dynamic_window; + constexpr double Eps = 1e-3; + + // function to compute dynamic window for a single dimension + auto compute_window = [&](const double & current_vel, const double & max_vel, + const double & min_vel, const double & max_accel, const double & max_decel) + { + double candidate_max_vel = 0.0; + double candidate_min_vel = 0.0; + + if (current_vel > Eps) { + // if the current velocity is positive, acceleration means an increase in speed + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel + max_decel * dt; + } else if (current_vel < -Eps) { + // if the current velocity is negative, acceleration means a decrease in speed + candidate_max_vel = current_vel - max_decel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } else { + // if the current velocity is zero, allow acceleration in both directions. + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } + + // clip to max/min velocity limits + double dynamic_window_max_vel = std::min(candidate_max_vel, max_vel); + double dynamic_window_min_vel = std::max(candidate_min_vel, min_vel); + return std::make_tuple(dynamic_window_max_vel, dynamic_window_min_vel); + }; + + // linear velocity + std::tie(dynamic_window.max_linear_vel, dynamic_window.min_linear_vel) = + compute_window( + current_speed.linear.x, max_linear_vel, min_linear_vel, + max_linear_accel, max_linear_decel); + + // angular velocity + std::tie(dynamic_window.max_angular_vel, dynamic_window.min_angular_vel) = + compute_window( + current_speed.angular.z, max_angular_vel, min_angular_vel, + max_angular_accel, max_angular_decel); + + return dynamic_window; +} + +/** + * @brief Apply regulated linear velocity to the dynamic window + * @param regulated_linear_vel Regulated linear velocity + * @param dynamic_window Dynamic window to be regulated + */ +inline void applyRegulationToDynamicWindow( + const double & regulated_linear_vel, + DynamicWindowBounds & dynamic_window) +{ + // Create regulated bounds [0, v_reg] or [v_reg, 0] + double v_reg_min = std::min(0.0, regulated_linear_vel); + double v_reg_max = std::max(0.0, regulated_linear_vel); + + // Intersect the dynamic window with the regulated bounds + dynamic_window.min_linear_vel = std::max(dynamic_window.min_linear_vel, v_reg_min); + dynamic_window.max_linear_vel = std::min(dynamic_window.max_linear_vel, v_reg_max); + + // If min > max, collapse to the nearest boundary + if (dynamic_window.min_linear_vel > dynamic_window.max_linear_vel) { + if (dynamic_window.min_linear_vel > v_reg_max) { + dynamic_window.max_linear_vel = dynamic_window.min_linear_vel; + } else { + dynamic_window.min_linear_vel = dynamic_window.max_linear_vel; + } + } +} + +/** + * @brief Compute the optimal velocity to follow the path within the dynamic window + * @param dynamic_window Dynamic window defining feasible velocity bounds + * @param curvature Curvature of the path to follow + * @param sign Velocity sign (forward or backward) + * @return Optimal linear and angular velocity + */ +inline std::tuple computeOptimalVelocityWithinDynamicWindow( + const DynamicWindowBounds & dynamic_window, + const double & curvature, + const double & sign +) +{ + double optimal_linear_vel; + double optimal_angular_vel; + + // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) + // Select the closest point to the line + // angular_vel = curvature * linear_vel within the dynamic window. + // If multiple points are equally close, select the one with the largest linear_vel. + + // When curvature == 0, the line is angular_vel = 0 + if (std::abs(curvature) < 1e-3) { + // linear velocity + if (sign >= 0.0) { + // If moving forward, select the max linear vel + optimal_linear_vel = dynamic_window.max_linear_vel; + } else { + // If moving backward, select the min linear vel + optimal_linear_vel = dynamic_window.min_linear_vel; + } + + // angular velocity + // If the line angular_vel = 0 intersects the dynamic window,angular_vel = 0.0 + if (dynamic_window.min_angular_vel <= 0.0 && 0.0 <= dynamic_window.max_angular_vel) { + optimal_angular_vel = 0.0; + } else { + // If not, select angular vel within dynamic window closest to 0 + if (std::abs(dynamic_window.min_angular_vel) <= std::abs(dynamic_window.max_angular_vel)) { + optimal_angular_vel = dynamic_window.min_angular_vel; + } else { + optimal_angular_vel = dynamic_window.max_angular_vel; + } + } + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); + } + + // When the dynamic window and the line angular_vel = curvature * linear_vel intersect, + // select the intersection point that yields the highest linear velocity. + + // List the four candidate intersection points + std::pair candidates[] = { + {dynamic_window.min_linear_vel, curvature * dynamic_window.min_linear_vel}, + {dynamic_window.max_linear_vel, curvature * dynamic_window.max_linear_vel}, + {dynamic_window.min_angular_vel / curvature, dynamic_window.min_angular_vel}, + {dynamic_window.max_angular_vel / curvature, dynamic_window.max_angular_vel} + }; + + double best_linear_vel = -std::numeric_limits::max() * sign; + double best_angular_vel = 0.0; + + for (auto [linear_vel, angular_vel] : candidates) { + // Check whether the candidate lies within the dynamic window + if (linear_vel >= dynamic_window.min_linear_vel && + linear_vel <= dynamic_window.max_linear_vel && + angular_vel >= dynamic_window.min_angular_vel && + angular_vel <= dynamic_window.max_angular_vel) + { + // Select the candidate with the largest linear velocity (considering moving direction) + if (linear_vel * sign > best_linear_vel * sign) { + best_linear_vel = linear_vel; + best_angular_vel = angular_vel; + } + } + } + + // If best_linear_vel was updated, it means that a valid intersection exists + if (best_linear_vel != -std::numeric_limits::max() * sign) { + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); + } + + // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection, + // select the point within the dynamic window that is closest to the line. + + // Because the dynamic window is a convex region, + // the closest point must be one of its four corners. + const std::array, 4> corners = { + { + {dynamic_window.min_linear_vel, dynamic_window.min_angular_vel}, + {dynamic_window.min_linear_vel, dynamic_window.max_angular_vel}, + {dynamic_window.max_linear_vel, dynamic_window.min_angular_vel}, + {dynamic_window.max_linear_vel, dynamic_window.max_angular_vel} + }}; + + // Compute the distance from a point (linear_vel, angular_vel) + // to the line angular_vel = curvature * linear_vel + const double denom = std::sqrt(curvature * curvature + 1.0); + auto compute_dist = [&](const std::array & corner) -> double { + return std::abs(curvature * corner[0] - corner[1]) / denom; + }; + + double closest_dist = std::numeric_limits::max(); + best_linear_vel = -std::numeric_limits::max() * sign; + best_angular_vel = 0.0; + + for (const auto & corner : corners) { + const double dist = compute_dist(corner); + // Update if this corner is closer to the line, + // or equally close but has a larger linear velocity (considering moving direction) + if (dist < closest_dist || + (std::abs(dist - closest_dist) <= 1e-3 && corner[0] * sign > best_linear_vel * sign)) + { + closest_dist = dist; + best_linear_vel = corner[0]; + best_angular_vel = corner[1]; + } + } + + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; + + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); +} + +/** + * @brief Compute velocity commands using Dynamic Window Pure Pursuit method + * @param current_speed Current linear and angular velocity of the robot + * @param max_linear_vel Maximum allowable linear velocity + * @param min_linear_vel Minimum allowable linear velocity + * @param max_angular_vel Maximum allowable angular velocity + * @param min_angular_vel Minimum allowable angular velocity + * @param max_linear_accel Maximum allowable linear acceleration + * @param max_linear_decel Maximum allowable linear deceleration + * @param max_angular_accel Maximum allowable angular acceleration + * @param max_angular_decel Maximum allowable angular deceleration + * @param regulated_linear_vel Regulated linear velocity + * @param curvature Curvature of the path to follow + * @param sign Velocity sign (forward or backward) + * @param dt Control duration + * @return Optimal linear and angular velocity + */ +inline std::tuple computeDynamicWindowVelocities( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & regulated_linear_vel, + const double & curvature, + const double & sign, + const double & dt +) +{ + // compute Dynamic Window + DynamicWindowBounds dynamic_window = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, dt); + + // apply regulation to Dynamic Window + applyRegulationToDynamicWindow(regulated_linear_vel, dynamic_window); + + // compute optimal velocity within Dynamic Window + auto [linear_vel, angular_vel] = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + + return std::make_tuple(linear_vel, angular_vel); +} + +} // namespace dynamic_window_pure_pursuit + +} // namespace easynav + +#endif // EASYNAV_REGULATED_PP_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp new file mode 100644 index 00000000..b671d264 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp @@ -0,0 +1,147 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + +/// \file +/// \brief Pure regulation heuristics for the Regulated Pure Pursuit controller. +/// +/// These are the same regulation terms described in: +/// S. Macenski, S. Singh, F. Martin, J. Gines, "Regulated Pure Pursuit for Robot Path Tracking", +/// Autonomous Robots, 2023 (https://arxiv.org/abs/2305.20026), ported from +/// nav2_regulated_pure_pursuit_controller. Since EasyNav controllers have no costmap available, +/// the cost-based regulation term is replaced by an equivalent one driven by the distance to the +/// nearest point in the fused point-cloud perception (see \ref easynav::RegulatedPurePursuitController). + +#ifndef EASYNAV_REGULATED_PP_CONTROLLER__REGULATION_FUNCTIONS_HPP_ +#define EASYNAV_REGULATED_PP_CONTROLLER__REGULATION_FUNCTIONS_HPP_ + +#include +#include +#include + +namespace easynav +{ + +namespace heuristics +{ + +/// \brief Curvature of the arc from the robot (at the local origin, facing +x) to a lookahead point. +/// \param lookahead_x X coordinate of the lookahead point in the robot frame. +/// \param lookahead_y Y coordinate of the lookahead point in the robot frame. +/// \return Signed curvature (1 / turning radius) of the arc. +inline double calculateCurvature(double lookahead_x, double lookahead_y) +{ + const double carrot_dist2 = lookahead_x * lookahead_x + lookahead_y * lookahead_y; + if (carrot_dist2 > 0.001) { + return 2.0 * lookahead_y / carrot_dist2; + } else { + return 0.0; + } +} + +/// \brief Apply curvature-based regulation on the linear velocity. +/// +/// Slows the robot down when the path curvature radius is smaller than \p min_radius, +/// so sharp turns are taken more conservatively. +/// \param raw_linear_vel Desired linear velocity before regulation. +/// \param curvature Curvature of the path at the regulation lookahead point. +/// \param min_radius Turning radius below which regulation kicks in. +/// \return Regulated linear velocity. +inline double curvatureConstraint( + const double raw_linear_vel, const double curvature, const double min_radius) +{ + const double radius = std::fabs(1.0 / curvature); + if (radius < min_radius) { + return raw_linear_vel * (1.0 - (std::fabs(radius - min_radius) / min_radius)); + } else { + return raw_linear_vel; + } +} + +/// \brief Apply obstacle-proximity regulation on the linear velocity. +/// +/// EasyNav controllers do not have access to a costmap, so this heuristic replaces +/// nav2_regulated_pure_pursuit_controller's cost-based regulation with an equivalent term based on +/// the distance to the closest obstacle point observed by the robot's perception. +/// \param raw_linear_vel Desired linear velocity before regulation. +/// \param min_obstacle_distance Distance (m) to the closest known obstacle ahead of the robot. +/// \param obstacle_scaling_dist Distance below which the regulation is triggered. +/// \param obstacle_scaling_gain Gain (<= 1.0) applied when scaling down the velocity. +/// \return Regulated linear velocity. +inline double obstacleConstraint( + const double raw_linear_vel, + const double min_obstacle_distance, + const double obstacle_scaling_dist, + const double obstacle_scaling_gain) +{ + if (!std::isfinite(min_obstacle_distance)) { + return raw_linear_vel; + } + + if (min_obstacle_distance < obstacle_scaling_dist) { + const double clamped_dist = std::max(0.0, min_obstacle_distance); + return raw_linear_vel * (obstacle_scaling_gain * clamped_dist / obstacle_scaling_dist); + } + + return raw_linear_vel; +} + +/// \brief Compute the scale factor to apply for linear velocity regulation on approach to goal. +/// \param remaining_path_distance Integrated distance left to travel along the path. +/// \param euclidean_dist_to_goal Straight-line distance from the robot to the goal. +/// \param approach_velocity_scaling_dist Distance away from goal at which to apply the heuristic. +/// \return A scale factor in [0.0, 1.0]. +inline double approachVelocityScalingFactor( + const double remaining_path_distance, + const double euclidean_dist_to_goal, + const double approach_velocity_scaling_dist) +{ + // Waiting to apply the threshold based on integrated distance ensures we don't erroneously + // apply approach scaling on curvy paths still far from the goal in a straight line. + if (remaining_path_distance < approach_velocity_scaling_dist) { + return euclidean_dist_to_goal / approach_velocity_scaling_dist; + } else { + return 1.0; + } +} + +/// \brief Velocity on approach to goal heuristic regulation term. +/// \param constrained_linear_vel Linear velocity already constrained by other heuristics. +/// \param remaining_path_distance Integrated distance left to travel along the path. +/// \param euclidean_dist_to_goal Straight-line distance from the robot to the goal. +/// \param min_approach_velocity Minimum velocity to use on approach to goal. +/// \param approach_velocity_scaling_dist Distance away from goal at which to apply the heuristic. +/// \return Velocity after regulation via approach-to-goal slow-down. +inline double approachVelocityConstraint( + const double constrained_linear_vel, + const double remaining_path_distance, + const double euclidean_dist_to_goal, + const double min_approach_velocity, + const double approach_velocity_scaling_dist) +{ + const double velocity_scaling = approachVelocityScalingFactor( + remaining_path_distance, euclidean_dist_to_goal, approach_velocity_scaling_dist); + double approach_vel = constrained_linear_vel * velocity_scaling; + + if (approach_vel < min_approach_velocity) { + approach_vel = min_approach_velocity; + } + + return std::min(constrained_linear_vel, approach_vel); +} + +} // namespace heuristics + +} // namespace easynav + +#endif // EASYNAV_REGULATED_PP_CONTROLLER__REGULATION_FUNCTIONS_HPP_ diff --git a/controllers/easynav_regulated_pp_controller/package.xml b/controllers/easynav_regulated_pp_controller/package.xml new file mode 100644 index 00000000..f3a604d6 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/package.xml @@ -0,0 +1,32 @@ + + + + easynav_regulated_pp_controller + 0.0.1 + Easy Navigation: port of the Nav2 Regulated Pure Pursuit Controller. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + easynav_system + easynav_sensors + pluginlib + tf2 + tf2_geometry_msgs + tf2_ros + geometry_msgs + nav_msgs + std_msgs + + rclcpp_lifecycle + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp new file mode 100644 index 00000000..dec983f6 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp @@ -0,0 +1,531 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +/// \file +/// \brief Implementation of the RegulatedPurePursuitController class. + +#include +#include +#include + +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp" +#include "easynav_regulated_pp_controller/regulation_functions.hpp" +#include "easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp" + +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_system/GoalManager.hpp" + +namespace easynav +{ + +namespace +{ +double normalizeAngle(double angle) +{ + while (angle > M_PI) {angle -= 2.0 * M_PI;} + while (angle < -M_PI) {angle += 2.0 * M_PI;} + return angle; +} +} // namespace + +RegulatedPurePursuitController::RegulatedPurePursuitController() {} + +RegulatedPurePursuitController::~RegulatedPurePursuitController() = default; + +void +RegulatedPurePursuitController::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + auto declare_and_get = [&node, &plugin_name](const std::string & name, auto & value) { + node->declare_parameter(plugin_name + "." + name, value); + node->get_parameter(plugin_name + "." + name, value); + }; + + declare_and_get("lookahead_dist", lookahead_dist_); + declare_and_get("min_lookahead_dist", min_lookahead_dist_); + declare_and_get("max_lookahead_dist", max_lookahead_dist_); + declare_and_get("lookahead_time", lookahead_time_); + declare_and_get("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_); + + declare_and_get("max_linear_vel", max_linear_vel_); + declare_and_get("min_linear_vel", min_linear_vel_); + declare_and_get("max_angular_vel", max_angular_vel_); + declare_and_get("min_angular_vel", min_angular_vel_); + declare_and_get("max_linear_accel", max_linear_accel_); + declare_and_get("max_linear_decel", max_linear_decel_); + declare_and_get("max_angular_accel", max_angular_accel_); + declare_and_get("max_angular_decel", max_angular_decel_); + declare_and_get("use_dynamic_window", use_dynamic_window_); + declare_and_get("allow_reversing", allow_reversing_); + + declare_and_get("use_rotate_to_heading", use_rotate_to_heading_); + declare_and_get("rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_); + declare_and_get("rotate_to_heading_min_angle", rotate_to_heading_min_angle_); + + declare_and_get("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_); + declare_and_get("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_); + declare_and_get("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_); + declare_and_get("use_fixed_curvature_lookahead", use_fixed_curvature_lookahead_); + declare_and_get("curvature_lookahead_dist", curvature_lookahead_dist_); + declare_and_get("interpolate_curvature_after_goal", interpolate_curvature_after_goal_); + + declare_and_get( + "use_obstacle_regulated_linear_velocity_scaling", + use_obstacle_regulated_linear_velocity_scaling_); + declare_and_get("obstacle_scaling_dist", obstacle_scaling_dist_); + declare_and_get("obstacle_scaling_gain", obstacle_scaling_gain_); + + declare_and_get("min_approach_linear_velocity", min_approach_linear_velocity_); + declare_and_get("approach_velocity_scaling_dist", approach_velocity_scaling_dist_); + + declare_and_get("xy_goal_tolerance", xy_goal_tolerance_); + declare_and_get("yaw_goal_tolerance", yaw_goal_tolerance_); + + lookahead_point_pub_ = node->create_publisher( + "lookahead_point", 10); + curvature_lookahead_point_pub_ = node->create_publisher( + "curvature_lookahead_point", 10); + is_rotating_to_heading_pub_ = node->create_publisher( + "is_rotating_to_heading", 10); + + last_update_ts_ = node->now(); +} + +void +RegulatedPurePursuitController::stop(NavState & nav_state, const std_msgs::msg::Header & header) +{ + cmd_vel_.header = header; + cmd_vel_.twist.linear.x = 0.0; + cmd_vel_.twist.angular.z = 0.0; + last_linear_vel_ = 0.0; + last_angular_vel_ = 0.0; + is_rotating_to_heading_ = false; + last_update_ts_ = get_node()->now(); + nav_state.set("cmd_vel", cmd_vel_); +} + +double +RegulatedPurePursuitController::getLookAheadDistance(double linear_vel) const +{ + double dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) { + dist = std::fabs(linear_vel) * lookahead_time_; + dist = std::clamp(dist, min_lookahead_dist_, max_lookahead_dist_); + } + return dist; +} + +geometry_msgs::msg::Point +RegulatedPurePursuitController::circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r, + const geometry_msgs::msg::Point & center) +{ + const double x1 = p1.x - center.x; + const double y1 = p1.y - center.y; + const double x2 = p2.x - center.x; + const double y2 = p2.y - center.y; + + const double dx = x2 - x1; + const double dy = y2 - y1; + const double dr2 = dx * dx + dy * dy; + + geometry_msgs::msg::Point result; + + if (dr2 < 1e-9) { + result = p2; + return result; + } + + const double D = x1 * y2 - x2 * y1; + const double discriminant = r * r * dr2 - D * D; + + if (discriminant < 0.0) { + // No real intersection (can happen with degenerate/noisy paths): fall back to p2. + result = p2; + return result; + } + + const double sqrt_term = std::sqrt(discriminant); + const double sign_dy = (dy < 0.0) ? -1.0 : 1.0; + + const double sol1_x = (D * dy + sign_dy * dx * sqrt_term) / dr2; + const double sol1_y = (-D * dx + std::fabs(dy) * sqrt_term) / dr2; + const double sol2_x = (D * dy - sign_dy * dx * sqrt_term) / dr2; + const double sol2_y = (-D * dx - std::fabs(dy) * sqrt_term) / dr2; + + // Choose the solution further along the segment direction (closer to p2). + const double t1 = ((sol1_x - x1) * dx + (sol1_y - y1) * dy) / dr2; + const double t2 = ((sol2_x - x1) * dx + (sol2_y - y1) * dy) / dr2; + + if (t1 >= t2) { + result.x = sol1_x + center.x; + result.y = sol1_y + center.y; + } else { + result.x = sol2_x + center.x; + result.y = sol2_y + center.y; + } + result.z = 0.0; + return result; +} + +std::size_t +RegulatedPurePursuitController::findClosestPoseIndex( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position) +{ + std::size_t closest_idx = 0; + double closest_dist = std::numeric_limits::max(); + for (std::size_t i = 0; i < path.poses.size(); ++i) { + const auto & p = path.poses[i].pose.position; + const double d = std::hypot(p.x - robot_position.x, p.y - robot_position.y); + if (d < closest_dist) { + closest_dist = d; + closest_idx = i; + } + } + return closest_idx; +} + +geometry_msgs::msg::Point +RegulatedPurePursuitController::getLookAheadPoint( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position, + double lookahead_dist, + bool interpolate_after_end) +{ + auto dist_to_robot = [&robot_position](const geometry_msgs::msg::Point & p) { + return std::hypot(p.x - robot_position.x, p.y - robot_position.y); + }; + + // Only look forward from the pose closest to the robot: the controller receives the full, + // un-pruned global path, so searching from index 0 would (incorrectly) match the path's start + // pose as soon as the robot has travelled more than a lookahead distance away from it, freezing + // the carrot behind the robot for the rest of the path. + const std::size_t start_idx = findClosestPoseIndex(path, robot_position); + const auto begin_it = path.poses.begin() + static_cast(start_idx); + + auto it = std::find_if( + begin_it, path.poses.end(), + [&](const geometry_msgs::msg::PoseStamped & ps) { + return dist_to_robot(ps.pose.position) >= lookahead_dist; + }); + + if (it == path.poses.end()) { + const auto & goal = path.poses.back().pose.position; + if (interpolate_after_end && path.poses.size() >= 2 && dist_to_robot(goal) < lookahead_dist) { + const auto & prev = path.poses[path.poses.size() - 2].pose.position; + const double seg_len = std::hypot(goal.x - prev.x, goal.y - prev.y); + if (seg_len > 1e-6) { + geometry_msgs::msg::Point extended; + extended.x = goal.x + (goal.x - prev.x) / seg_len * lookahead_dist; + extended.y = goal.y + (goal.y - prev.y) / seg_len * lookahead_dist; + return circleSegmentIntersection(goal, extended, lookahead_dist, robot_position); + } + } + return goal; + } + + if (it == begin_it) { + return it->pose.position; + } + + const auto prev_it = std::prev(it); + return circleSegmentIntersection( + prev_it->pose.position, it->pose.position, lookahead_dist, robot_position); +} + +geometry_msgs::msg::Point +RegulatedPurePursuitController::toRobotFrame( + const geometry_msgs::msg::Point & point, + const geometry_msgs::msg::Pose & robot_pose, + double robot_yaw) +{ + const double dx = point.x - robot_pose.position.x; + const double dy = point.y - robot_pose.position.y; + const double c = std::cos(-robot_yaw); + const double s = std::sin(-robot_yaw); + + geometry_msgs::msg::Point local; + local.x = c * dx - s * dy; + local.y = s * dx + c * dy; + local.z = 0.0; + return local; +} + +bool +RegulatedPurePursuitController::shouldRotateToPath(double angle_to_path) const +{ + return use_rotate_to_heading_ && std::fabs(angle_to_path) > rotate_to_heading_min_angle_; +} + +void +RegulatedPurePursuitController::rotateToHeading( + double & linear_vel, double & angular_vel, double angle_to_target, double dt) const +{ + linear_vel = 0.0; + const double sign = angle_to_target > 0.0 ? 1.0 : -1.0; + angular_vel = sign * rotate_to_heading_angular_vel_; + + const double min_feasible = last_angular_vel_ - max_angular_accel_ * dt; + const double max_feasible = last_angular_vel_ + max_angular_accel_ * dt; + angular_vel = std::clamp(angular_vel, min_feasible, max_feasible); + + // Slow down to avoid overshooting the target angle. + const double max_vel_to_stop = std::sqrt(2.0 * max_angular_accel_ * std::fabs(angle_to_target)); + if (std::fabs(angular_vel) > max_vel_to_stop) { + angular_vel = sign * max_vel_to_stop; + } +} + +void +RegulatedPurePursuitController::applyConstraints( + double curvature, + double min_obstacle_distance, + double remaining_path_distance, + double euclidean_dist_to_goal, + double & linear_vel) const +{ + double curvature_vel = linear_vel; + double obstacle_vel = linear_vel; + + if (use_regulated_linear_velocity_scaling_) { + curvature_vel = heuristics::curvatureConstraint( + linear_vel, curvature, regulated_linear_scaling_min_radius_); + } + + if (use_obstacle_regulated_linear_velocity_scaling_) { + obstacle_vel = heuristics::obstacleConstraint( + linear_vel, min_obstacle_distance, obstacle_scaling_dist_, obstacle_scaling_gain_); + } + + linear_vel = std::min(curvature_vel, obstacle_vel); + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + + linear_vel = heuristics::approachVelocityConstraint( + linear_vel, remaining_path_distance, euclidean_dist_to_goal, + min_approach_linear_velocity_, approach_velocity_scaling_dist_); +} + +double +RegulatedPurePursuitController::computeMinObstacleDistance( + NavState & nav_state, double max_range) const +{ + const auto & perceptions = nav_state.get_by_type(); + if (perceptions.empty()) { + return std::numeric_limits::infinity(); + } + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + const auto & robot_frame = tf_info.robot_frame; + + auto view = PointPerceptionsOpsView(perceptions); + view.fuse(robot_frame) + .filter( + {0.0, -robot_radius_ - safety_margin_, z_min_filter_}, + {std::max(max_range, 0.0), robot_radius_ + safety_margin_, robot_height_}); + + const auto & cloud = view.as_points(); + + double min_dist = std::numeric_limits::infinity(); + for (const auto & p : cloud.points) { + if (!std::isfinite(p.x) || !std::isfinite(p.y)) {continue;} + const double d = std::hypot(p.x, p.y) - robot_radius_; + if (d < min_dist) {min_dist = d;} + } + + return min_dist; +} + +double +RegulatedPurePursuitController::remainingPathDistance( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Point & robot_position) +{ + if (path.poses.empty()) {return 0.0;} + + const std::size_t closest_idx = findClosestPoseIndex(path, robot_position); + const auto & closest_p = path.poses[closest_idx].pose.position; + double distance = std::hypot(closest_p.x - robot_position.x, closest_p.y - robot_position.y); + for (std::size_t i = closest_idx + 1; i < path.poses.size(); ++i) { + const auto & prev = path.poses[i - 1].pose.position; + const auto & cur = path.poses[i].pose.position; + distance += std::hypot(cur.x - prev.x, cur.y - prev.y); + } + + return distance; +} + +void +RegulatedPurePursuitController::publishCarrot( + const rclcpp::Publisher::SharedPtr & pub, + const geometry_msgs::msg::Point & carrot, + const std_msgs::msg::Header & header) const +{ + if (!pub || pub->get_subscription_count() == 0) {return;} + geometry_msgs::msg::PointStamped msg; + msg.header = header; + msg.point = carrot; + msg.point.z = 0.05; + pub->publish(msg); +} + +void +RegulatedPurePursuitController::update_rt(NavState & nav_state) +{ + if (nav_state.has("navigation_state")) { + const auto goal_state = nav_state.get("navigation_state"); + if (goal_state == easynav::GoalManager::State::IDLE) { + std_msgs::msg::Header header; + header.stamp = get_node()->now(); + stop(nav_state, header); + return; + } + } + + if (!nav_state.has("path") || !nav_state.has("robot_pose")) {return;} + + const auto & path = nav_state.get("path"); + + std_msgs::msg::Header header; + header.frame_id = path.header.frame_id; + header.stamp = get_node()->now(); + + if (path.poses.empty()) { + stop(nav_state, header); + return; + } + + const auto & robot_pose = nav_state.get("robot_pose").pose.pose; + const double robot_yaw = tf2::getYaw(robot_pose.orientation); + + const auto & goal_pose = path.poses.back().pose; + + double xy_tol = xy_goal_tolerance_; + double yaw_tol = yaw_goal_tolerance_; + if (nav_state.has("goal_tolerance.position")) { + xy_tol = nav_state.get("goal_tolerance.position"); + } + if (nav_state.has("goal_tolerance.yaw")) { + yaw_tol = nav_state.get("goal_tolerance.yaw"); + } + + const double dist_to_goal = std::hypot( + goal_pose.position.x - robot_pose.position.x, + goal_pose.position.y - robot_pose.position.y); + const double yaw_goal = tf2::getYaw(goal_pose.orientation); + const double e_theta_goal = normalizeAngle(yaw_goal - robot_yaw); + + const bool at_goal_xy = dist_to_goal <= xy_tol; + + if (at_goal_xy && std::fabs(e_theta_goal) <= yaw_tol) { + stop(nav_state, header); + return; + } + + double dt = (get_node()->now() - last_update_ts_).seconds(); + if (dt <= 0.0 || dt > 1.0) {dt = 0.05;} + last_update_ts_ = get_node()->now(); + + double linear_vel = max_linear_vel_; + double angular_vel = 0.0; + + if (use_rotate_to_heading_ && at_goal_xy) { + is_rotating_to_heading_ = true; + rotateToHeading(linear_vel, angular_vel, e_theta_goal, dt); + } else { + const double lookahead_dist = getLookAheadDistance(last_linear_vel_); + const auto carrot_global = getLookAheadPoint(path, robot_pose.position, lookahead_dist); + const auto carrot_local = toRobotFrame(carrot_global, robot_pose, robot_yaw); + publishCarrot(lookahead_point_pub_, carrot_global, header); + + auto curvature_local = carrot_local; + if (use_fixed_curvature_lookahead_) { + const auto curvature_carrot_global = getLookAheadPoint( + path, robot_pose.position, curvature_lookahead_dist_, interpolate_curvature_after_goal_); + curvature_local = toRobotFrame(curvature_carrot_global, robot_pose, robot_yaw); + publishCarrot(curvature_lookahead_point_pub_, curvature_carrot_global, header); + } + + double x_vel_sign = 1.0; + if (allow_reversing_) { + x_vel_sign = carrot_local.x >= 0.0 ? 1.0 : -1.0; + } + + double angle_to_path = std::atan2(curvature_local.y, curvature_local.x); + if (x_vel_sign < 0.0) { + angle_to_path = normalizeAngle(angle_to_path + M_PI); + } + + const double regulation_curvature = heuristics::calculateCurvature( + curvature_local.x, curvature_local.y); + + if (shouldRotateToPath(angle_to_path)) { + is_rotating_to_heading_ = true; + rotateToHeading(linear_vel, angular_vel, angle_to_path, dt); + } else { + is_rotating_to_heading_ = false; + + const double min_obstacle_distance = use_obstacle_regulated_linear_velocity_scaling_ ? + computeMinObstacleDistance(nav_state, lookahead_dist) : + std::numeric_limits::infinity(); + const double remaining_path_distance = remainingPathDistance(path, robot_pose.position); + + applyConstraints( + regulation_curvature, min_obstacle_distance, remaining_path_distance, dist_to_goal, + linear_vel); + linear_vel = x_vel_sign * std::clamp(std::fabs(linear_vel), 0.0, max_linear_vel_); + + if (!use_dynamic_window_) { + angular_vel = linear_vel * regulation_curvature; + } else { + geometry_msgs::msg::Twist current_speed; + current_speed.linear.x = last_linear_vel_; + current_speed.angular.z = last_angular_vel_; + std::tie(linear_vel, angular_vel) = + dynamic_window_pure_pursuit::computeDynamicWindowVelocities( + current_speed, max_linear_vel_, min_linear_vel_, max_angular_vel_, min_angular_vel_, + max_linear_accel_, max_linear_decel_, max_angular_accel_, max_angular_decel_, + linear_vel, regulation_curvature, x_vel_sign, dt); + } + } + } + + last_linear_vel_ = linear_vel; + last_angular_vel_ = angular_vel; + + if (is_rotating_to_heading_pub_ && is_rotating_to_heading_pub_->get_subscription_count() > 0) { + std_msgs::msg::Bool msg; + msg.data = is_rotating_to_heading_; + is_rotating_to_heading_pub_->publish(msg); + } + + cmd_vel_.header = header; + cmd_vel_.twist.linear.x = linear_vel; + cmd_vel_.twist.angular.z = angular_vel; + nav_state.set("cmd_vel", cmd_vel_); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS( + easynav::RegulatedPurePursuitController, easynav::ControllerMethodBase) diff --git a/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt b/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt new file mode 100644 index 00000000..7c7c867f --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt @@ -0,0 +1,4 @@ +ament_add_gtest(regulated_pp_controller_tests regulated_pp_controller_tests.cpp) +target_link_libraries(regulated_pp_controller_tests + ${PROJECT_NAME} +) diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp new file mode 100644 index 00000000..a9bc6ee6 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp @@ -0,0 +1,257 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 + +#include +#include + +#include "easynav_regulated_pp_controller/regulation_functions.hpp" +#include "easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp" +#include "easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp" + +using easynav::heuristics::approachVelocityConstraint; +using easynav::heuristics::calculateCurvature; +using easynav::heuristics::curvatureConstraint; +using easynav::heuristics::obstacleConstraint; + +TEST(RegulationFunctions, CalculateCurvatureStraightAhead) +{ + // A lookahead point directly ahead has zero lateral offset -> zero curvature. + EXPECT_NEAR(calculateCurvature(1.0, 0.0), 0.0, 1e-6); +} + +TEST(RegulationFunctions, CalculateCurvatureLateralOffset) +{ + // k = 2y / (x^2+y^2) + const double k = calculateCurvature(1.0, 0.5); + EXPECT_NEAR(k, 2.0 * 0.5 / (1.0 * 1.0 + 0.5 * 0.5), 1e-9); +} + +TEST(RegulationFunctions, CalculateCurvatureDegenerate) +{ + EXPECT_NEAR(calculateCurvature(0.0, 0.0), 0.0, 1e-9); +} + +TEST(RegulationFunctions, CurvatureConstraintNoRegulationOnGentleTurn) +{ + // Radius = 1/curvature = 10 m, well above the 0.9 m min radius: no regulation applied. + const double v = curvatureConstraint(1.0, 0.1, 0.9); + EXPECT_NEAR(v, 1.0, 1e-9); +} + +TEST(RegulationFunctions, CurvatureConstraintSlowsOnSharpTurn) +{ + // Radius = 1/2.0 = 0.5 m, below the 1.0 m min radius: velocity should be reduced. + const double v = curvatureConstraint(1.0, 2.0, 1.0); + EXPECT_LT(v, 1.0); + EXPECT_GT(v, 0.0); +} + +TEST(RegulationFunctions, ObstacleConstraintNoObstacleNearby) +{ + const double v = obstacleConstraint(1.0, 5.0, 0.3, 1.0); + EXPECT_NEAR(v, 1.0, 1e-9); +} + +TEST(RegulationFunctions, ObstacleConstraintSlowsNearObstacle) +{ + const double v = obstacleConstraint(1.0, 0.1, 0.3, 1.0); + EXPECT_LT(v, 1.0); + EXPECT_GT(v, 0.0); +} + +TEST(RegulationFunctions, ObstacleConstraintIgnoresInfiniteDistance) +{ + const double v = obstacleConstraint(1.0, std::numeric_limits::infinity(), 0.3, 1.0); + EXPECT_NEAR(v, 1.0, 1e-9); +} + +TEST(RegulationFunctions, ApproachVelocityConstraintFarFromGoal) +{ + const double v = approachVelocityConstraint(1.0, 10.0, 10.0, 0.05, 1.0); + EXPECT_NEAR(v, 1.0, 1e-9); +} + +TEST(RegulationFunctions, ApproachVelocityConstraintNearGoal) +{ + const double v = approachVelocityConstraint(1.0, 0.5, 0.5, 0.05, 1.0); + EXPECT_LT(v, 1.0); + EXPECT_GE(v, 0.05); +} + +TEST(DynamicWindowPurePursuit, ComputeDynamicWindowClampsToAccelLimits) +{ + geometry_msgs::msg::Twist current_speed; + current_speed.linear.x = 0.0; + current_speed.angular.z = 0.0; + + const auto window = easynav::dynamic_window_pure_pursuit::computeDynamicWindow( + current_speed, /*max_linear_vel=*/1.0, /*min_linear_vel=*/-1.0, + /*max_angular_vel=*/1.0, /*min_angular_vel=*/-1.0, + /*max_linear_accel=*/2.0, /*max_linear_decel=*/2.0, + /*max_angular_accel=*/2.0, /*max_angular_decel=*/2.0, /*dt=*/0.1); + + EXPECT_NEAR(window.max_linear_vel, 0.2, 1e-9); + EXPECT_NEAR(window.min_linear_vel, -0.2, 1e-9); +} + +TEST(DynamicWindowPurePursuit, ComputeOptimalVelocityZeroCurvatureForward) +{ + easynav::dynamic_window_pure_pursuit::DynamicWindowBounds window; + window.max_linear_vel = 1.0; + window.min_linear_vel = -1.0; + window.max_angular_vel = 1.0; + window.min_angular_vel = -1.0; + + const auto [lin, ang] = + easynav::dynamic_window_pure_pursuit::computeOptimalVelocityWithinDynamicWindow( + window, /*curvature=*/0.0, /*sign=*/1.0); + + EXPECT_NEAR(lin, 1.0, 1e-9); + EXPECT_NEAR(ang, 0.0, 1e-9); +} + +class FriendRegulatedPurePursuitController : public easynav::RegulatedPurePursuitController +{ +public: + using easynav::RegulatedPurePursuitController::getLookAheadPoint; + using easynav::RegulatedPurePursuitController::circleSegmentIntersection; + using easynav::RegulatedPurePursuitController::toRobotFrame; + using easynav::RegulatedPurePursuitController::remainingPathDistance; + using easynav::RegulatedPurePursuitController::findClosestPoseIndex; +}; + +TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointInterpolatesOnSegment) +{ + nav_msgs::msg::Path path; + geometry_msgs::msg::PoseStamped p0, p1, p2; + p0.pose.position.x = 0.0; + p1.pose.position.x = 1.0; + p2.pose.position.x = 2.0; + path.poses = {p0, p1, p2}; + + geometry_msgs::msg::Point robot; + robot.x = 0.0; + robot.y = 0.0; + + const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( + path, robot, /*lookahead_dist=*/1.5); + + EXPECT_NEAR(carrot.x, 1.5, 1e-6); + EXPECT_NEAR(carrot.y, 0.0, 1e-6); +} + +TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointIgnoresPathBehindRobot) +{ + // Regression test: the controller receives the full, un-pruned global path, so the carrot + // search must start from the pose closest to the robot, not from the path's start pose. A path + // start pose that is far behind the robot must never be picked up as the carrot. + nav_msgs::msg::Path path; + for (double x = 0.0; x <= 10.0; x += 0.1) { + geometry_msgs::msg::PoseStamped p; + p.pose.position.x = x; + path.poses.push_back(p); + } + + // Robot has already travelled most of the path. + geometry_msgs::msg::Point robot; + robot.x = 9.0; + robot.y = 0.0; + + const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( + path, robot, /*lookahead_dist=*/0.5); + + // The carrot must be ahead of the robot (close to x=9.5), never back at the path's start (x=0). + EXPECT_NEAR(carrot.x, 9.5, 1e-6); + EXPECT_NEAR(carrot.y, 0.0, 1e-6); +} + +TEST(RegulatedPurePursuitControllerHelpers, FindClosestPoseIndexPicksNearestNotFirst) +{ + nav_msgs::msg::Path path; + for (double x = 0.0; x <= 10.0; x += 1.0) { + geometry_msgs::msg::PoseStamped p; + p.pose.position.x = x; + path.poses.push_back(p); + } + + geometry_msgs::msg::Point robot; + robot.x = 7.1; + robot.y = 0.0; + + const auto idx = FriendRegulatedPurePursuitController::findClosestPoseIndex(path, robot); + EXPECT_EQ(idx, 7u); +} + +TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointClampsToPathEnd) +{ + nav_msgs::msg::Path path; + geometry_msgs::msg::PoseStamped p0, p1; + p0.pose.position.x = 0.0; + p1.pose.position.x = 1.0; + path.poses = {p0, p1}; + + geometry_msgs::msg::Point robot; + robot.x = 0.0; + robot.y = 0.0; + + const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( + path, robot, /*lookahead_dist=*/5.0); + + EXPECT_NEAR(carrot.x, 1.0, 1e-6); +} + +TEST(RegulatedPurePursuitControllerHelpers, ToRobotFrameRotatesAndTranslates) +{ + geometry_msgs::msg::Point global_point; + global_point.x = 1.0; + global_point.y = 1.0; + + geometry_msgs::msg::Pose robot_pose; + robot_pose.position.x = 1.0; + robot_pose.position.y = 0.0; + + // Robot facing +90 degrees (looking along +y): the point straight ahead in the global + // frame (0, 1) relative to the robot should map to local +x. + const auto local = FriendRegulatedPurePursuitController::toRobotFrame( + global_point, robot_pose, M_PI_2); + + EXPECT_NEAR(local.x, 1.0, 1e-6); + EXPECT_NEAR(local.y, 0.0, 1e-6); +} + +TEST(RegulatedPurePursuitControllerHelpers, RemainingPathDistanceFromClosestPoint) +{ + nav_msgs::msg::Path path; + geometry_msgs::msg::PoseStamped p0, p1, p2; + p0.pose.position.x = 0.0; + p1.pose.position.x = 1.0; + p2.pose.position.x = 2.0; + path.poses = {p0, p1, p2}; + + geometry_msgs::msg::Point robot; + robot.x = 1.0; + robot.y = 0.0; + + const double dist = FriendRegulatedPurePursuitController::remainingPathDistance(path, robot); + EXPECT_NEAR(dist, 1.0, 1e-6); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From fa1cc7e0b110f011d3e03805a33473c8a6d605b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 17 Jul 2026 07:44:04 +0200 Subject: [PATCH 196/230] Fix lcense and copyright MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../RegulatedPurePursuitController.hpp | 5 +++++ .../regulation_functions.hpp | 6 ++++++ controllers/easynav_regulated_pp_controller/package.xml | 3 +++ .../RegulatedPurePursuitController.cpp | 5 +++++ 4 files changed, 19 insertions(+) diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp index 4c4e8d36..68259b84 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -1,6 +1,11 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) +// and is a derivative work of nav2_regulated_pure_pursuit_controller, +// ported to the EasyNav plugin architecture. +// // 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 diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp index b671d264..7902a918 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp @@ -1,5 +1,11 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America // Copyright 2025 Intelligent Robotics Lab // +// This file is part of the project Easy Navigation (EasyNav in short) +// and is a derivative work of nav2_regulated_pure_pursuit_controller, +// ported to the EasyNav plugin architecture. +// // 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 diff --git a/controllers/easynav_regulated_pp_controller/package.xml b/controllers/easynav_regulated_pp_controller/package.xml index f3a604d6..c1609bc4 100644 --- a/controllers/easynav_regulated_pp_controller/package.xml +++ b/controllers/easynav_regulated_pp_controller/package.xml @@ -7,6 +7,9 @@ Francisco Martín Rico Apache-2.0 + Steve Macenski + Shrijit Singh + ament_cmake easynav_common diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp index dec983f6..efdf1e92 100644 --- a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +++ b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp @@ -1,6 +1,11 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America // Copyright 2025 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) +// and is a derivative work of nav2_regulated_pure_pursuit_controller, +// ported to the EasyNav plugin architecture. +// // 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 From 693194a023138b2063962edf2e9c51abd3bec540 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 17 Jul 2026 07:46:50 +0200 Subject: [PATCH 197/230] Fix lcense and copyright MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../RegulatedPurePursuitController.hpp | 2 +- .../dynamic_window_pure_pursuit_functions.hpp | 2 +- .../easynav_regulated_pp_controller/regulation_functions.hpp | 2 +- .../RegulatedPurePursuitController.cpp | 2 +- .../tests/regulated_pp_controller_tests.cpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp index 68259b84..607e2075 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -1,6 +1,6 @@ // Copyright (c) 2020 Shrijit Singh // Copyright (c) 2020 Samsung Research America -// Copyright 2025 Intelligent Robotics Lab +// Copyright 2026 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) // and is a derivative work of nav2_regulated_pure_pursuit_controller, diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp index 381bc708..bb6cde6a 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp @@ -1,5 +1,5 @@ // Copyright (c) 2025 Fumiya Ohnishi -// Copyright 2025 Intelligent Robotics Lab +// Copyright 2026 Intelligent Robotics Lab // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp index 7902a918..babc2859 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp @@ -1,6 +1,6 @@ // Copyright (c) 2020 Shrijit Singh // Copyright (c) 2020 Samsung Research America -// Copyright 2025 Intelligent Robotics Lab +// Copyright 2026 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) // and is a derivative work of nav2_regulated_pure_pursuit_controller, diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp index efdf1e92..7e3f6443 100644 --- a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +++ b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp @@ -1,6 +1,6 @@ // Copyright (c) 2020 Shrijit Singh // Copyright (c) 2020 Samsung Research America -// Copyright 2025 Intelligent Robotics Lab +// Copyright 2026 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) // and is a derivative work of nav2_regulated_pure_pursuit_controller, diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp index a9bc6ee6..467b1465 100644 --- a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 Intelligent Robotics Lab +// Copyright 2026 Intelligent Robotics Lab // // This file is part of the project Easy Navigation (EasyNav in short) // Licensed under the Apache License, Version 2.0 (the "License"); From 9711d7a04ecaf3856c419b0c1338d76b1a04333e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 18 Jul 2026 19:31:37 +0200 Subject: [PATCH 198/230] Add temporally robot_localization to thirdparty MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/thirdparty.repos | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index c4abe867..8fc6a74a 100644 --- a/.github/thirdparty.repos +++ b/.github/thirdparty.repos @@ -11,3 +11,11 @@ repositories: type: git url: https://github.com/EasyNavigation/NavMap.git version: rolling + # robot_localization has no released .deb for rolling yet (buildfarm + # job is failing despite the rosdistro entry), so it is built from + # source here. rosdep --ignore-src will skip it once it's in src/. + # Remove this entry once ros-rolling-robot-localization is installable again. + ThirdParty/robot_localization: + type: git + url: https://github.com/cra-ros-pkg/robot_localization.git + version: rolling-devel From 2a7b1d142844705c4dcecf05f005e1882a9cedcd Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Mon, 6 Jul 2026 11:57:17 +0200 Subject: [PATCH 199/230] Fix rolling CI --- .github/workflows/rolling.yaml | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index d70f2b65..cafefba5 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -12,18 +12,16 @@ on: workflow_dispatch: jobs: build-and-test: - runs-on: ubuntu-24.04 + runs-on: ubuntu-26.04 container: - image: ubuntu:noble + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-rolling-ros-base:master steps: - name: Repo checkout - uses: actions/checkout@v6-beta + uses: actions/checkout@v6 with: ref: rolling - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 + uses: ros-tooling/action-ros-ci@0.4.8 with: package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: rolling From 0f8ff72f3a22881595a4b2f48d88abd170b2d2f5 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Mon, 6 Jul 2026 12:23:19 +0200 Subject: [PATCH 200/230] Update calls to deprecated get_package_share_directory --- .../src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp | 6 +++--- .../src/easynav_costmap_maps_manager/CostmapMapsManager.cpp | 4 ++-- .../src/easynav_navmap_maps_manager/NavMapMapsManager.cpp | 6 +++--- .../src/easynav_octomap_maps_manager/OctomapMapsManager.cpp | 6 +++--- .../src/easynav_routes_maps_manager/RoutesMapsManager.cpp | 6 +++--- .../src/easynav_simple_maps_manager/SimpleMapsManager.cpp | 4 ++-- 6 files changed, 16 insertions(+), 16 deletions(-) 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 b5222471..4ad4cfb4 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 @@ -27,7 +27,7 @@ #include "pcl/point_types.h" #include "pcl/point_cloud.h" -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav_bonxai @@ -73,7 +73,7 @@ BonxaiMapsManager::on_initialize() if (!package_name.empty() && !bonxai_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + bonxai_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); @@ -106,7 +106,7 @@ BonxaiMapsManager::on_initialize() if (!package_name.empty() && !occmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 66be2125..3f8507e4 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 @@ -24,7 +24,7 @@ #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_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav @@ -110,7 +110,7 @@ CostmapMapsManager::on_initialize() map_path_ = "/tmp/default.map.yaml"; if (!package_name.empty() && !map_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + map_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 aa0d04e4..2e9bdf48 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 @@ -26,7 +26,7 @@ #include "navmap_ros/navmap_io.hpp" #include "easynav_navmap_maps_manager/map_io.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav @@ -122,7 +122,7 @@ NavMapMapsManager::on_initialize() if (!package_name.empty() && !occmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); @@ -145,7 +145,7 @@ NavMapMapsManager::on_initialize() if (!package_name.empty() && !navmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); + const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + std::string("/") + navmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 e8e1a6c6..c79fd50a 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 @@ -29,7 +29,7 @@ #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_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav @@ -113,7 +113,7 @@ OctomapMapsManager::on_initialize() // if (!package_name.empty() && !occmap_path_file.empty()) { // try { -// const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); +// const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { // throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); @@ -136,7 +136,7 @@ OctomapMapsManager::on_initialize() // // if (!package_name.empty() && !octomap_path_file.empty()) { // try { -// const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); +// const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { // throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 3c0645f3..d12884bf 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,7 @@ #include -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_share_path.hpp" #include "rclcpp/rclcpp.hpp" @@ -84,8 +84,8 @@ void RoutesMapsManager::on_initialize() // 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; + const auto pkgpath = ament_index_cpp::get_package_share_path(package_name); + map_path_ = pkgpath / map_path_file; } else { throw std::runtime_error( "Parameters '" + plugin_name + ".package' and '" + plugin_name + 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 76751621..2681b6ea 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 @@ -19,7 +19,7 @@ #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "easynav_sensors/types/PointPerception.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" +#include "ament_index_cpp/get_package_share_path.hpp" #include "ament_index_cpp/get_package_prefix.hpp" #include "easynav_common/YTSession.hpp" @@ -62,7 +62,7 @@ SimpleMapsManager::on_initialize() if (package_name != "" && map_path_file != "") { std::string pkgpath; try { - pkgpath = ament_index_cpp::get_package_share_directory(package_name); + pkgpath = ament_index_cpp::get_package_share_path(package_name); map_path_ = pkgpath + "/" + map_path_file; } catch(ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); From 36a76ea926231d7407aa11a53d966656280a7c62 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Tue, 7 Jul 2026 22:29:09 +0200 Subject: [PATCH 201/230] Check if std is non-positive before creating std::normal_distribution --- .../AMCLLocalizer.cpp | 14 +++++++++++++ .../tests/costmap_localizer_tests.cpp | 20 +++++++++---------- .../AMCLLocalizer.cpp | 16 +++++++++++++++ .../tests/navmap_localizer_tests.cpp | 20 +++++++++---------- .../AMCLLocalizer.cpp | 15 ++++++++++++++ .../tests/simple_localizer_tests.cpp | 20 +++++++++---------- 6 files changed, 75 insertions(+), 30 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 d229c8cb..033cc8b6 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -224,6 +224,20 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); reseed_time_ = 1.0 / reseed_freq; + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles); RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index 6259a598..0ce41b2e 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -79,10 +79,10 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), rclcpp::Parameter("test.initial_pose.yaw", yaw0), - rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), - rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), - rclcpp::Parameter("test.min_noise_xy", 0.0), - rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), rclcpp::Parameter("test.compute_odom_from_tf", true), }); @@ -95,16 +95,16 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG { const tf2::Transform tf = localizer->getEstimatedPose(); - EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); - EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); - EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); } { const nav_msgs::msg::Odometry odom = localizer->get_pose(); - EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); - EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); - EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); } const auto infos = node->get_subscriptions_info_by_topic("initialpose"); 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 2b6dc641..fe3b726f 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -415,6 +415,22 @@ void AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".top_keep_fraction", top_keep_fraction_); node->get_parameter(plugin_name + ".downsampled_cloud_size", downsampled_cloud_size_); + + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + + if (!std::isfinite(inflation_stddev_) || inflation_stddev_ <= 0.0) {inflation_stddev_ = 1.5;} if (!std::isfinite(inflation_prob_min_) || inflation_prob_min_ <= 0.0) { inflation_prob_min_ = 0.01; diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp index 89dfed8a..720c89f7 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp @@ -77,10 +77,10 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), rclcpp::Parameter("test.initial_pose.yaw", yaw0), - rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), - rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), - rclcpp::Parameter("test.min_noise_xy", 0.0), - rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), rclcpp::Parameter("test.compute_odom_from_tf", true), }); @@ -94,16 +94,16 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG { const tf2::Transform tf = localizer->getEstimatedPose(); - EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); - EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); - EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); } { const nav_msgs::msg::Odometry odom = localizer->get_pose(); - EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); - EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); - EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); } const auto infos = node->get_subscriptions_info_by_topic("initialpose"); 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 d391cb4b..3f1dbb1e 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -216,6 +216,21 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); + // Check if any of the standard deviations are non-positive + // This is undefined behavior in std::normal_distribution and may result in a runtime assertion + if ( + std_dev_xy <= 0.0 || + std_dev_yaw <= 0.0 || + noise_translation_ <= 0.0 || + noise_rotation_ <= 0.0 || + noise_translation_to_rotation_ <= 0.0 || + min_noise_xy_ <= 0.0 || + min_noise_yaw_ <= 0.0) + { + throw std::runtime_error("AMCLLocalizer: standard deviations must be positive"); + } + + double reseed_freq; node->get_parameter(plugin_name + ".reseed_freq", reseed_freq); reseed_time_ = 1.0 / reseed_freq; diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index 36efd008..12dd1636 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -77,10 +77,10 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), rclcpp::Parameter("test.initial_pose.yaw", yaw0), - rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), - rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), - rclcpp::Parameter("test.min_noise_xy", 0.0), - rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 1e-12), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 1e-12), + rclcpp::Parameter("test.min_noise_xy", 1e-12), + rclcpp::Parameter("test.min_noise_yaw", 1e-12), }); auto node = std::make_shared( @@ -93,16 +93,16 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG { const tf2::Transform tf = localizer->getEstimatedPose(); - EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); - EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); - EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-6); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-6); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-6); } { const nav_msgs::msg::Odometry odom = localizer->get_pose(); - EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); - EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); - EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-6); } const auto infos = node->get_subscriptions_info_by_topic("initialpose"); From 8a7a41ad5551b18c8263f7b14a66f5c6edc2147a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 18 Jul 2026 19:31:37 +0200 Subject: [PATCH 202/230] Add temporally robot_localization to thirdparty MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/thirdparty.repos | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index c4abe867..8fc6a74a 100644 --- a/.github/thirdparty.repos +++ b/.github/thirdparty.repos @@ -11,3 +11,11 @@ repositories: type: git url: https://github.com/EasyNavigation/NavMap.git version: rolling + # robot_localization has no released .deb for rolling yet (buildfarm + # job is failing despite the rosdistro entry), so it is built from + # source here. rosdep --ignore-src will skip it once it's in src/. + # Remove this entry once ros-rolling-robot-localization is installable again. + ThirdParty/robot_localization: + type: git + url: https://github.com/cra-ros-pkg/robot_localization.git + version: rolling-devel From 51f8e23b83b8278cf10440e92fe7d46e028e90fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 3 May 2026 09:51:45 +0200 Subject: [PATCH 203/230] Cells cost have no real impact in path creation 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 | 9 ++++----- 1 file changed, 4 insertions(+), 5 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 f6bfdf20..702f5be8 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -140,7 +140,7 @@ void CostmapPlanner::on_initialize() void CostmapPlanner::update(NavState & nav_state) { - if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { + if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map")) { return; } @@ -150,7 +150,7 @@ void CostmapPlanner::update(NavState & nav_state) return; } - const auto & map = nav_state.get("map.dynamic"); + const auto & map = nav_state.get("map"); 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(); @@ -264,7 +264,6 @@ 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 = 1.0; const double diagonal_cost = std::sqrt(2.0); auto idx = [&](int x, int y) {return y * width + x;}; @@ -296,8 +295,8 @@ std::vector CostmapPlanner::a_star_path( // 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_norm; + // Calculate traversal cost: cost_factor_ acts as a direct multiplier on cell cost + double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost); 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; From d5eefe059105419aac335fc6550e01b1a6ef5691 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 3 May 2026 09:53:30 +0200 Subject: [PATCH 204/230] Navstate key among filters are always map, not an arbitrary key MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../SerestController.cpp | 2 +- localizers/easynav_simple_localizer/README.md | 2 +- .../easynav_simple_localizer/AMCLLocalizer.cpp | 6 +++--- .../easynav_costmap_maps_manager/README.md | 14 +++++++------- .../CostmapMapsManager.cpp | 4 ++-- .../filters/InflationFilter.cpp | 8 ++++---- .../filters/ObstacleFilter.cpp | 4 ++-- .../tests/costmap_mapsmanager_tests.cpp | 16 ++++++++-------- .../tests/navmap_mapsmanager_tests.cpp | 8 ++++---- .../easynav_routes_maps_manager/README.md | 4 ++-- .../filters/RoutesCostmapFilter.hpp | 4 ++-- .../filters/RoutesCostmapFilter.cpp | 4 ++-- .../tests/routes_costmap_filter_tests.cpp | 10 +++++----- .../easynav_simple_maps_manager/README.md | 4 ++-- .../SimpleMapsManager.cpp | 8 ++++---- .../tests/simple_mapsmanager_tests.cpp | 8 ++++---- planners/easynav_costmap_planner/README.md | 2 +- planners/easynav_simple_planner/README.md | 2 +- .../src/easynav_simple_planner/SimplePlanner.cpp | 10 +++++----- 19 files changed, 60 insertions(+), 60 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 9a32f1ff..52272e5f 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -375,7 +375,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")) { + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map")) { publish_stop(nav_state, tf_info.robot_footprint_frame); return false; } diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 8a06b1be..428f59cf 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -65,7 +65,7 @@ This package does not create service servers or clients. | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. | -| `map.static` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | +| `map.base` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Estimated robot pose. | ## TF Frames 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 3f1dbb1e..23729b9c 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -542,12 +542,12 @@ void AMCLLocalizer::correct(NavState & nav_state) return; } - if (!nav_state.has("map.static")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); + if (!nav_state.has("map.base")) { + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map"); return; } - const auto & map_static = nav_state.get("map.static"); + const auto & map_static = nav_state.get("map.base"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 3de9cd38..67e6c9a0 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -72,8 +72,8 @@ Each entry in `.filters` defines a sub-namespace `.` wit | 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. | +| `map` | `Costmap2D` | **Write** | Marks cells as `LETHAL_OBSTACLE` (254). | +| `map.obstacle_bounds` | `ObstacleBounds` | **Write** | Bounding box of updated obstacles for incremental inflation. | #### **InflationFilter** @@ -96,8 +96,8 @@ Each entry in `.filters` defines a sub-namespace `.` wit | Key | Type | Access | Description | |---|---|---|---| | `map.base` | `Costmap2D` | **Read** | Base 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. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic costmap input and output after inflation. | +| `map.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. @@ -146,9 +146,9 @@ maps_manager_node: | Key | Type | Access | Notes | |---|---|---|---| | `map.base` | `Costmap2D` | **Write** | Base 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). | +| `map` | `Costmap2D` | **Write** | Dynamic map after applying filters. | +| `map` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | +| `map.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | --- 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 3f8507e4..0010a116 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 @@ -218,7 +218,7 @@ CostmapMapsManager::update(NavState & nav_state) *dynamic_map_ = map_base_; } - nav_state.set("map.dynamic.filtered", dynamic_map_); + nav_state.set("map", dynamic_map_); if (!nav_state.has("map_time")) { nav_state.set("map_time", get_node()->now()); @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map", dynamic_map_); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 4df61c69..f03c72d1 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,7 +99,7 @@ InflationFilter::on_initialize() void InflationFilter::update(NavState & nav_state) { - auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map"); Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & base_map = nav_state.get("map.base"); @@ -121,8 +121,8 @@ InflationFilter::update(NavState & nav_state) int max_i = size_x; int max_j = size_y; - if (nav_state.has("map.dynamic.obstacle_bounds")) { - const auto & bb = nav_state.get("map.dynamic.obstacle_bounds"); + if (nav_state.has("map.obstacle_bounds")) { + const auto & bb = nav_state.get("map.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) && @@ -154,7 +154,7 @@ InflationFilter::update(NavState & nav_state) } } - nav_state.set("map.dynamic.filtered", dynamic_map_ptr); + nav_state.set("map", dynamic_map_ptr); } 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 69e41672..d29fab15 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 @@ -47,7 +47,7 @@ ObstacleFilter::update(NavState & nav_state) return; } - auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map"); Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -81,7 +81,7 @@ ObstacleFilter::update(NavState & nav_state) if (!fused.empty()) { ObstacleBounds bb{min_x, min_y, max_x, max_y}; - nav_state.set("map.dynamic.obstacle_bounds", bb); + nav_state.set("map.obstacle_bounds", bb); } } 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 19cf79db..d962bfd4 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 @@ -103,8 +103,8 @@ TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerNavStateMap) const auto & base_after = navstate.get("map.base"); EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); EXPECT_EQ(dyn_ptr->getCost(2, 2), easynav::LETHAL_OBSTACLE); @@ -142,8 +142,8 @@ TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerInternalMap) const auto & base_after = navstate.get("map.base"); EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); EXPECT_EQ(dyn_ptr->getCost(3, 3), easynav::LETHAL_OBSTACLE); @@ -187,8 +187,8 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) EXPECT_EQ(base_after.getCost(5, 5), easynav::LETHAL_OBSTACLE); EXPECT_GT(base_after.getLastModifiedStamp().nanoseconds(), 0); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getCost(5, 5), easynav::LETHAL_OBSTACLE); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), @@ -224,8 +224,8 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) // // manager->update(navstate); // -// ASSERT_TRUE(navstate.has("map.dynamic")); -// const auto & map = navstate.get("map.dynamic"); +// ASSERT_TRUE(navstate.has("map")); +// const auto & map = navstate.get("map"); // // unsigned int cx, cy; // ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); 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 4c55b8fe..913234a8 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 @@ -68,8 +68,8 @@ class NavMapMapsManagerTest : public ::testing::Test // // manager->update(::easynav::NavState); // -// ASSERT_TRUE(::easynav::NavState.has("map.dynamic")); -// const auto & map = ::easynav::NavState.get("map.dynamic"); +// ASSERT_TRUE(::easynav::NavState.has("map")); +// const auto & map = ::easynav::NavState.get("map"); // // unsigned int cx, cy; // ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); @@ -111,8 +111,8 @@ class NavMapMapsManagerTest : public ::testing::Test // ::easynav::NavState ::easynav::NavState; // manager->update(::easynav::NavState); // -// ASSERT_TRUE(::easynav::NavState.has("map.static")); -// const auto & map = ::easynav::NavState.get("map.static"); +// ASSERT_TRUE(::easynav::NavState.has("map.base")); +// const auto & map = ::easynav::NavState.get("map.base"); // // EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); // EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md index 1bf0af58..3e1ba4f2 100644 --- a/maps_managers/easynav_routes_maps_manager/README.md +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -97,7 +97,7 @@ has been written to the NavState on each update cycle. - **Plugin Name:** `easynav_routes_maps_manager/RoutesCostmapFilter` - **Type:** `easynav::RoutesCostmapFilter` - **Description:** - Reads the current `RoutesMap` and a dynamic costmap (`map.dynamic.filtered`) + Reads the current `RoutesMap` and a dynamic costmap (`map`) 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. @@ -114,7 +114,7 @@ has been written to the NavState on each update cycle. | 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`. | +| `map` | `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 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 7ea65635..975883ba 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 @@ -28,7 +28,7 @@ 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 +/// ("map") 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. @@ -56,7 +56,7 @@ class RoutesCostmapFilter : public RoutesFilter /// @brief Apply the routes-based filtering to the costmap. /// - /// If both "routes" and "map.dynamic.filtered" entries are present + /// If both "routes" and "map" 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. 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 10a2a897..e5fb7b2d 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 @@ -73,13 +73,13 @@ RoutesCostmapFilter::update(NavState & nav_state) return; } - if (!nav_state.has("map.dynamic.filtered")) { + if (!nav_state.has("map")) { return; } const auto & routes = nav_state.get("routes"); - auto costmap_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto costmap_ptr = nav_state.get_ptr("map"); if (!costmap_ptr) { return; } 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 a6f574ca..011e462e 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 @@ -58,7 +58,7 @@ TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); NavState nav_state; - // No 'routes' and no 'map.dynamic.filtered' keys -> update should not crash + // No 'routes' and no 'map' keys -> update should not crash EXPECT_NO_THROW(filter.update(nav_state)); } @@ -93,11 +93,11 @@ TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) NavState nav_state; nav_state.set("routes", routes); - nav_state.set("map.dynamic.filtered", map); + nav_state.set("map", map); filter.update(nav_state); - auto map_after = nav_state.get("map.dynamic.filtered"); + auto map_after = nav_state.get("map"); // Cells clearly along the interior of the route should remain 0, // others should be >= 50 (default min_cost). The last endpoint @@ -142,11 +142,11 @@ TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) NavState nav_state; nav_state.set("routes", routes); - nav_state.set("map.dynamic.filtered", map); + nav_state.set("map", map); filter.update(nav_state); - auto map_after = nav_state.get("map.dynamic.filtered"); + auto map_after = nav_state.get("map"); // No cell should be considered "on route"; all should be raised to >= 50 for (unsigned int x = 0; x < 5; ++x) { diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 10f59ab4..00755612 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -58,8 +58,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. | +| `map.base` | `SimpleMap` | **Write** | Static map loaded from file / parameter configuration. | +| `map` | `SimpleMap` | **Write** | Dynamic map after applying incoming updates. | ## TF Frames 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 2681b6ea..2980211b 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 @@ -144,8 +144,8 @@ SimpleMapsManager::update(NavState & nav_state) } if (perceptions.empty()) { - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map.base", static_map_); + nav_state.set("map", dynamic_map_); return; } @@ -166,8 +166,8 @@ SimpleMapsManager::update(NavState & nav_state) } } - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map.base", static_map_); + nav_state.set("map", dynamic_map_); dynamic_map_.to_occupancy_grid(dynamic_grid_msg_); dynamic_grid_msg_.header.frame_id = tf_info.map_frame; 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 e6adb415..449dee29 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 @@ -99,8 +99,8 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) manager->update(navstate); - ASSERT_TRUE(navstate.has("map.dynamic")); - auto map = navstate.get("map.dynamic"); + ASSERT_TRUE(navstate.has("map")); + auto map = navstate.get("map"); auto cell1 = map.metric_to_cell(1.0, 1.0); EXPECT_TRUE(map.at(cell1.first, cell1.second)); @@ -148,8 +148,8 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) manager->update(navstate); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ASSERT_TRUE(navstate.has("map.static")); - const auto & map = navstate.get("map.static"); + ASSERT_TRUE(navstate.has("map.base")); + const auto & map = navstate.get("map.base"); EXPECT_EQ(map.at(5, 5), 1); EXPECT_EQ(map.at(1, 1), 0); diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index 6750fca3..df142bd3 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -96,7 +96,7 @@ This plugin does not create subscriptions or services directly; it reads inputs | Key | Type | Access | Notes | |---|---|---|---| | `goals` | `nav_msgs::msg::Goals` | **Read** | Goal list used as planner targets. | -| `map.dynamic` | `Costmap2D` | **Read** | Dynamic costmap used for A* search. | +| `map` | `Costmap2D` | **Read** | Dynamic costmap used for A* search. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose used as the start position. | | `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | diff --git a/planners/easynav_simple_planner/README.md b/planners/easynav_simple_planner/README.md index 324dc632..efe694cc 100644 --- a/planners/easynav_simple_planner/README.md +++ b/planners/easynav_simple_planner/README.md @@ -47,7 +47,7 @@ This plugin does not create subscriptions or services directly; it reads inputs | Key | Type | Access | Notes | |---|---|---|---| | `goals` | `nav_msgs::msg::Goals` | **Read** | Planner targets. | -| `map.dynamic` | `SimpleMap` | **Read** | Dynamic `SimpleMap` grid used for search. | +| `map` | `SimpleMap` | **Read** | Dynamic `SimpleMap` grid used for search. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Start pose for path planning. | | `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | 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 b869c68c..aa8f707d 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -113,16 +113,16 @@ SimplePlanner::update(NavState & nav_state) return; } - if (!nav_state.has("map.dynamic")) { - RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update map.dynamic map not found"); + if (!nav_state.has("map")) { + RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update map map not found"); return; } SimpleMap map_typed; - if (nav_state.has("map.dynamic")) { - map_typed = nav_state.get("map.dynamic"); + if (nav_state.has("map")) { + map_typed = nav_state.get("map"); } else { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.dynamic map"); + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map"); return; } From b0a0ad081f284db288a07361ebdb9f22d911d52e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:24:29 +0200 Subject: [PATCH 205/230] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- maps_managers/easynav_costmap_maps_manager/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 67e6c9a0..7acf6015 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -146,8 +146,7 @@ maps_manager_node: | Key | Type | Access | Notes | |---|---|---|---| | `map.base` | `Costmap2D` | **Write** | Base map loaded from YAML. | -| `map` | `Costmap2D` | **Write** | Dynamic map after applying filters. | -| `map` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic map after applying filters; if an existing filtered `map` is available, the manager may use it as input. | | `map.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | --- From 0863b24ae44302c2ec0a61da8d2cc279f89bb1a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:24:56 +0200 Subject: [PATCH 206/230] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- .../easynav_costmap_maps_manager/filters/ObstacleFilter.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 d29fab15..dfce79fa 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 @@ -48,6 +48,10 @@ ObstacleFilter::update(NavState & nav_state) } auto dynamic_map_ptr = nav_state.get_ptr("map"); + if (!dynamic_map_ptr) { + RCLCPP_WARN(get_node()->get_logger(), "There is no map in NavState"); + return; + } Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); From dc0801b19465e140dd8eafe3d18706360f03cc93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:29:14 +0200 Subject: [PATCH 207/230] Avoid rewrite map with dynamic_map 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 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0010a116..404abe2d 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 @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - nav_state.set("map", dynamic_map_); + dynamic_map_ = nav_state.get>("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); From 8f72a470462c929c5d4bf2fa18f81a15d62b13ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 5 May 2026 07:40:33 +0200 Subject: [PATCH 208/230] Update dynamic_map_ with map key at navstate 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 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 404abe2d..bfab9799 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 @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - dynamic_map_ = nav_state.get>("map"); + *dynamic_map_ = nav_state.get("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); From a82f2aebdff0584a0a5627e8e5c57a62d0c1aeee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 19 Jul 2026 10:08:19 +0200 Subject: [PATCH 209/230] Update Readme 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 8d9c02fb..059c9a47 100644 --- a/README.md +++ b/README.md @@ -42,6 +42,7 @@ Motion controllers for trajectory tracking and reactive behaviors. | `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) | +| `easynav_regulated_pp_controller` | Regulated Pure Pursuit controller, with optional Dynamic Window (DWPP) extension. | [README](./controllers/easynav_regulated_pp_controller/README.md) | --- From 38f28e4b2b45f34b1c82353b6700fb99897948f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Jul 2026 10:47:48 +0200 Subject: [PATCH 210/230] Fix overshoot when DWPP is activated MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_regulated_pp_controller/README.md | 4 +++- .../dynamic_window_pure_pursuit_functions.hpp | 4 ++-- .../RegulatedPurePursuitController.cpp | 12 +++++++++--- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/controllers/easynav_regulated_pp_controller/README.md b/controllers/easynav_regulated_pp_controller/README.md index 7522dd45..03be5d46 100644 --- a/controllers/easynav_regulated_pp_controller/README.md +++ b/controllers/easynav_regulated_pp_controller/README.md @@ -161,7 +161,9 @@ Same guidance as upstream Nav2 applies: - For Adaptive Pure Pursuit behavior, disable all boolean regulation parameters except `use_velocity_scaled_lookahead_dist`, and tune `lookahead_time`, `min_lookahead_dist` and `max_lookahead_dist`. -- For plain Pure Pursuit behavior, disable all boolean parameters and tune `lookahead_dist`. +- For plain Pure Pursuit behavior, disable all boolean parameters and set + `approach_velocity_scaling_dist: 0.0` to disable approach-to-goal slow-down, then tune + `lookahead_dist`. - `use_obstacle_regulated_linear_velocity_scaling` can over-trigger in tightly cluttered environments (every point in a narrow corridor is "close"); tune `obstacle_scaling_dist` and `obstacle_scaling_gain` for the platform's environment, the same way `cost_scaling_dist` / diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp index bb6cde6a..3073370c 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp @@ -88,10 +88,10 @@ inline DynamicWindowBounds computeDynamicWindow( if (current_vel > Eps) { // if the current velocity is positive, acceleration means an increase in speed candidate_max_vel = current_vel + max_accel * dt; - candidate_min_vel = current_vel + max_decel * dt; + candidate_min_vel = current_vel - max_decel * dt; } else if (current_vel < -Eps) { // if the current velocity is negative, acceleration means a decrease in speed - candidate_max_vel = current_vel - max_decel * dt; + candidate_max_vel = current_vel + max_decel * dt; candidate_min_vel = current_vel - max_accel * dt; } else { // if the current velocity is zero, allow acceleration in both directions. diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp index 7e3f6443..7d78d1a5 100644 --- a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +++ b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp @@ -290,12 +290,18 @@ RegulatedPurePursuitController::rotateToHeading( const double sign = angle_to_target > 0.0 ? 1.0 : -1.0; angular_vel = sign * rotate_to_heading_angular_vel_; - const double min_feasible = last_angular_vel_ - max_angular_accel_ * dt; - const double max_feasible = last_angular_vel_ + max_angular_accel_ * dt; + // Braking (magnitude decreasing) is bounded by max_angular_decel_, speeding up + // (magnitude increasing) by max_angular_accel_; which bound applies to which side + // of the window depends on the current direction of rotation. + const double decel_bound = last_angular_vel_ >= 0.0 ? max_angular_decel_ : max_angular_accel_; + const double accel_bound = last_angular_vel_ >= 0.0 ? max_angular_accel_ : max_angular_decel_; + const double min_feasible = last_angular_vel_ - decel_bound * dt; + const double max_feasible = last_angular_vel_ + accel_bound * dt; angular_vel = std::clamp(angular_vel, min_feasible, max_feasible); // Slow down to avoid overshooting the target angle. - const double max_vel_to_stop = std::sqrt(2.0 * max_angular_accel_ * std::fabs(angle_to_target)); + const double decel = std::max(max_angular_decel_, 1e-9); + const double max_vel_to_stop = std::sqrt(2.0 * decel * std::fabs(angle_to_target)); if (std::fabs(angular_vel) > max_vel_to_stop) { angular_vel = sign * max_vel_to_stop; } From 248aadb2596605fcab861914a542555996e04398 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 23 Jul 2026 18:27:48 +0200 Subject: [PATCH 211/230] Fix std::normal_distribution when stdev is zero MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../AMCLLocalizer.cpp | 21 +++++++++++-------- .../AMCLLocalizer.cpp | 21 +++++++++++-------- 2 files changed, 24 insertions(+), 18 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 033cc8b6..12bcaca0 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -242,9 +242,9 @@ AMCLLocalizer::on_initialize() RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); + std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_y(y_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_yaw(yaw_init, std::max(std_dev_yaw, 1e-12)); particles_.resize(num_particles); for (auto & p : particles_) { @@ -430,7 +430,7 @@ AMCLLocalizer::init_pose_callback( 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); + std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, 1e-12)); const std::size_t N = particles_.size(); @@ -546,13 +546,16 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) for (auto & p : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); std::normal_distribution noise_yaw( 0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); tf2::Vector3 noisy_translation( dx + noise_dx(rng_), @@ -680,7 +683,7 @@ AMCLLocalizer::reseed() double yaw_variance = computeYawVariance(particles_, 0, N_top); double yaw_stddev = std::sqrt(yaw_variance); std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_)); - std::normal_distribution index_dist(0.0, 0.05 * static_cast(N)); + std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); std::normal_distribution standard_normal(0.0, 1.0); for (std::size_t i = N_top; i < N; ++i) { 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 23729b9c..62244a58 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -239,9 +239,9 @@ AMCLLocalizer::on_initialize() RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); + std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_y(y_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_yaw(yaw_init, std::max(std_dev_yaw, 1e-12)); particles_.resize(num_particles); for (auto & p : particles_) { @@ -425,7 +425,7 @@ AMCLLocalizer::init_pose_callback( 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); + std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, 1e-12)); const std::size_t N = particles_.size(); for (std::size_t i = 0; i < N; ++i) { @@ -503,13 +503,16 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) std::mt19937 gen(rd()); for (auto & p : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); std::normal_distribution noise_yaw( 0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); tf2::Vector3 noisy_translation( dx + noise_dx(gen), @@ -636,7 +639,7 @@ AMCLLocalizer::reseed() double yaw_variance = computeYawVariance(particles_, 0, N_top); double yaw_stddev = std::sqrt(yaw_variance); std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_)); - std::normal_distribution index_dist(0.0, 0.05 * static_cast(N)); + std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); std::normal_distribution standard_normal(0.0, 1.0); for (std::size_t i = N_top; i < N; ++i) { From 44a788f30b6b3a623cb396cf77b6f5bcae3e39a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 23 Jul 2026 18:59:58 +0200 Subject: [PATCH 212/230] Avoid creating objects every loop cycle MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../AMCLLocalizer.cpp | 21 +++++++++---------- .../AMCLLocalizer.cpp | 21 +++++++++---------- 2 files changed, 20 insertions(+), 22 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 12bcaca0..b07dfc2c 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -545,18 +545,17 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) double rot_len = std::abs(yaw); - for (auto & p : particles_) { - std::normal_distribution noise_dx( - 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); - std::normal_distribution noise_dy( - 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); - std::normal_distribution noise_dz( - 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); - - std::normal_distribution noise_yaw( - 0.0, - std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); + std::normal_distribution noise_yaw( + 0.0, + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + for (auto & p : particles_) { tf2::Vector3 noisy_translation( dx + noise_dx(rng_), dy + noise_dy(rng_), 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 62244a58..43c5edab 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -502,18 +502,17 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) std::random_device rd; std::mt19937 gen(rd()); - for (auto & p : particles_) { - std::normal_distribution noise_dx( - 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); - std::normal_distribution noise_dy( - 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); - std::normal_distribution noise_dz( - 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); - - std::normal_distribution noise_yaw( - 0.0, - std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); + std::normal_distribution noise_yaw( + 0.0, + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + for (auto & p : particles_) { tf2::Vector3 noisy_translation( dx + noise_dx(gen), dy + noise_dy(gen), From a1aa2ddbca9716955ee539d1bfda50acaeec2d88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 23 Jul 2026 19:45:11 +0200 Subject: [PATCH 213/230] Added CI for Lyrical MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/lyrical.yaml | 49 ++++++++++++++++++++++++++++++++++ README.md | 1 + 2 files changed, 50 insertions(+) create mode 100644 .github/workflows/lyrical.yaml diff --git a/.github/workflows/lyrical.yaml b/.github/workflows/lyrical.yaml new file mode 100644 index 00000000..8443a2fb --- /dev/null +++ b/.github/workflows/lyrical.yaml @@ -0,0 +1,49 @@ +name: lyrical + +on: + pull_request: + branches: + - lyrical + push: + branches: + - lyrical + schedule: + - cron: '0 0 * * 6' + workflow_dispatch: +jobs: + build-and-test: + runs-on: ubuntu-26.04 + container: + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-lyrical-ros-base:master + steps: + - name: Repo checkout + uses: actions/checkout@v6 + with: + ref: lyrical + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.8 + with: + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller + target-ros2-distro: lyrical + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/README.md b/README.md index 059c9a47..15cc761c 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,7 @@ [![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?branch=rolling)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/rolling.yaml) +[![lyrical](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/lyrical.yaml/badge.svg?branch=lyrical)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/lyrical.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) From 459c9a784717776f9b43174f31c251d4d4d519c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Jul 2026 09:59:55 +0200 Subject: [PATCH 214/230] Sync with rolling branch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp | 6 +++--- .../easynav_costmap_maps_manager/CostmapMapsManager.cpp | 4 ++-- .../src/easynav_navmap_maps_manager/NavMapMapsManager.cpp | 6 +++--- .../easynav_octomap_maps_manager/OctomapMapsManager.cpp | 2 +- .../src/easynav_routes_maps_manager/RoutesMapsManager.cpp | 8 +++++--- .../src/easynav_simple_maps_manager/SimpleMapsManager.cpp | 4 ++-- 6 files changed, 16 insertions(+), 14 deletions(-) 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 4ad4cfb4..b5222471 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 @@ -27,7 +27,7 @@ #include "pcl/point_types.h" #include "pcl/point_cloud.h" -#include "ament_index_cpp/get_package_share_path.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav_bonxai @@ -73,7 +73,7 @@ BonxaiMapsManager::on_initialize() if (!package_name.empty() && !bonxai_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); + 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) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); @@ -106,7 +106,7 @@ BonxaiMapsManager::on_initialize() if (!package_name.empty() && !occmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); + 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) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 bfab9799..f763f0a5 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 @@ -24,7 +24,7 @@ #include "easynav_costmap_maps_manager/map_io.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "ament_index_cpp/get_package_share_path.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav @@ -110,7 +110,7 @@ CostmapMapsManager::on_initialize() map_path_ = "/tmp/default.map.yaml"; if (!package_name.empty() && !map_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); + 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) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 2e9bdf48..aa0d04e4 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 @@ -26,7 +26,7 @@ #include "navmap_ros/navmap_io.hpp" #include "easynav_navmap_maps_manager/map_io.hpp" -#include "ament_index_cpp/get_package_share_path.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" namespace easynav @@ -122,7 +122,7 @@ NavMapMapsManager::on_initialize() if (!package_name.empty() && !occmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); + 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) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); @@ -145,7 +145,7 @@ NavMapMapsManager::on_initialize() if (!package_name.empty() && !navmap_path_file.empty()) { try { - const std::string pkgpath = ament_index_cpp::get_package_share_path(package_name); + 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) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); 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 c79fd50a..5faa02e8 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 @@ -29,7 +29,7 @@ #include "pcl/point_cloud.h" #include "pcl/filters/voxel_grid.h" -#include "ament_index_cpp/get_package_share_path.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" 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 d12884bf..4c073eb8 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,9 @@ #include -#include "ament_index_cpp/get_package_share_path.hpp" +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" #include "rclcpp/rclcpp.hpp" @@ -84,8 +86,8 @@ void RoutesMapsManager::on_initialize() // 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_path(package_name); - map_path_ = pkgpath / map_path_file; + const std::filesystem::path pkgpath(ament_index_cpp::get_package_share_directory(package_name)); + map_path_ = (pkgpath / map_path_file).string(); } else { throw std::runtime_error( "Parameters '" + plugin_name + ".package' and '" + plugin_name + 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 2980211b..a711d4cf 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 @@ -19,7 +19,7 @@ #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "easynav_sensors/types/PointPerception.hpp" -#include "ament_index_cpp/get_package_share_path.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" #include "easynav_common/YTSession.hpp" @@ -62,7 +62,7 @@ SimpleMapsManager::on_initialize() if (package_name != "" && map_path_file != "") { std::string pkgpath; try { - pkgpath = ament_index_cpp::get_package_share_path(package_name); + pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + "/" + map_path_file; } catch(ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); From cc6aded662c4c2ad0e939093b3930865ffb6514c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Jul 2026 10:16:22 +0200 Subject: [PATCH 215/230] Added CI for Lyrical 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 | 1 + .github/workflows/jazzy_cron.yaml | 1 + .github/workflows/kilted.yaml | 1 + .github/workflows/kilted_cron.yaml | 1 + 4 files changed, 4 insertions(+) diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index f12799f9..89695fac 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -25,6 +25,7 @@ jobs: with: package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy + 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 c7049a98..4fbccdb1 100644 --- a/.github/workflows/jazzy_cron.yaml +++ b/.github/workflows/jazzy_cron.yaml @@ -20,6 +20,7 @@ jobs: with: package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index ee4356be..2ca980a1 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -25,6 +25,7 @@ jobs: with: package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: kilted + 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 c4b6b82b..7a245495 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.github/workflows/kilted_cron.yaml @@ -20,6 +20,7 @@ jobs: with: package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: kilted + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { From 835c467e4f6209d36ca3e529847efefd1b16b612 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Jul 2026 11:02:36 +0200 Subject: [PATCH 216/230] Sync with Rolling branch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- common/easynav_costmap_common/package.xml | 4 - .../SerestController.cpp | 5 - .../easynav_vff_controller/VffController.cpp | 4 - .../easynav_costmap_localizer/package.xml | 4 - .../AMCLLocalizer.cpp | 22 ----- .../example_params_summit_dual_gazebo.yaml | 3 - .../FusionLocalizer.cpp | 64 ------------- localizers/easynav_gps_localizer/package.xml | 4 - .../PerceptionModel.hpp | 4 - .../easynav_navmap_localizer/package.xml | 4 - .../easynav_simple_localizer/package.xml | 4 - .../AMCLLocalizer.cpp | 4 - .../tests/simple_localizer_tests.cpp | 96 ------------------- .../filters/ObstacleFilter.cpp | 4 - .../filters/OctomapFilter.cpp | 4 - .../easynav_simple_maps_manager/package.xml | 4 - 16 files changed, 234 deletions(-) diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 56d1c395..2170fe49 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -2,11 +2,7 @@ easynav_costmap_common -<<<<<<< HEAD - 0.2.1 -======= 0.3.1 ->>>>>>> kilted Easy Navigation: Simple Common package. Francisco Martín Rico 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 8ae67416..9e894fd1 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -375,13 +375,8 @@ SerestController::fetch_required_inputs( { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); -<<<<<<< HEAD - if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { - publish_stop(nav_state, tf_info.robot_frame); -======= if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map")) { publish_stop(nav_state, tf_info.robot_footprint_frame); ->>>>>>> kilted return false; } 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 44fbb78d..fc39d89d 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -258,11 +258,7 @@ 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(tf_info.map_frame) -======= .fuse(tf_info.robot_frame) ->>>>>>> kilted .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_}) diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index b8ff3b26..bc8b00d0 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -2,11 +2,7 @@ easynav_costmap_localizer -<<<<<<< HEAD - 0.2.1 -======= 0.3.1 ->>>>>>> kilted Easy Navigation: Costmap Localizer package. Francisco Martín Rico Apache-2.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 e4a6478e..827e364c 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -599,11 +599,7 @@ AMCLLocalizer::correct(NavState & nav_state) auto view = PointPerceptionsOpsView(perceptions); view.downsample(map_static.getResolution()) -<<<<<<< HEAD - .fuse(tf_info.robot_frame) -======= .fuse(tf_info.robot_footprint_frame, last_input_time_) ->>>>>>> kilted .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.getResolution()); @@ -614,24 +610,6 @@ AMCLLocalizer::correct(NavState & nav_state) return; } -<<<<<<< HEAD - 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); - -======= ->>>>>>> kilted for (auto & particle : particles_) { int hits = 0; int possible_hits = 0; diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml index b5a8fc30..7ec21016 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml @@ -267,10 +267,7 @@ sensors_node: gps: topic: /gps/fix type: sensor_msgs/msg/NavSatFix -<<<<<<< HEAD -======= group: gnss ->>>>>>> kilted laser1: topic: /front_laser/points type: sensor_msgs/msg/PointCloud2 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 5fe77e73..8488f319 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -4,13 +4,8 @@ #include "easynav_common/RTTFBuffer.hpp" -<<<<<<< HEAD -#include "easynav_common/types/IMUPerception.hpp" -#include "easynav_common/types/GNSSPerception.hpp" -======= #include "easynav_sensors/types/IMUPerception.hpp" #include "easynav_sensors/types/GNSSPerception.hpp" ->>>>>>> kilted #include "sensor_msgs/msg/nav_sat_status.hpp" #include @@ -35,11 +30,8 @@ void FusionLocalizer::on_initialize() "initialpose", 10, std::bind(&FusionLocalizer::init_pose_callback, this, std::placeholders::_1)); -<<<<<<< HEAD -======= auto localizer_node = std::dynamic_pointer_cast(node); ->>>>>>> kilted const std::string & plugin_name = this->get_plugin_name(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -136,61 +128,6 @@ void FusionLocalizer::on_initialize() RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); } -<<<<<<< HEAD -void FusionLocalizer::update_rt(NavState & nav_state) -{ - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - - if (has_global_filter_) { - if (n_gps_sensors_ && nav_state.has("gnss")) { - auto gps_data = nav_state.get(std::string("gnss")); - const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); - for (int i = 0; i < n_gps_sensors_; ++i) { - if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { - continue; - } - rclcpp::Time gps_time(gps_data[i]->data.header.stamp); - if (gps_time > last_gps_stamp_[i]) { - EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); - last_gps_stamp_[i] = gps_time; - auto pose = - std::make_shared(navsatfix_to_pose( - gps_data[i]->data)); - if (!first_pose_received_) { - RCLCPP_INFO(get_node()->get_logger(), - "First valid GPS fix received. Initializing filter state."); - if(nav_state.has("imu")) { - auto imu_data = nav_state.get(std::string("imu")); - if (!imu_data.empty()) { - pose->pose.pose.orientation = imu_data[0]->data.orientation; - } - } - ukf_global_->setPoseCallback(pose); - first_pose_received_ = true; - continue; - } - ukf_global_->poseCallback( - pose, - gps_cb_arr[i], - tf_info.map_frame, - gps_data[i]->data.header.frame_id, - false - ); - } - } - } - - ukf_global_->periodicUpdate(); - - nav_msgs::msg::Odometry global_odom; - if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { - nav_state.set("robot_pose", global_odom); - navsat_pub_->publish(odom_to_navsatfix(global_odom)); - } - } - - if (has_local_filter_) { -======= void FusionLocalizer::init_pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { @@ -272,7 +209,6 @@ void FusionLocalizer::update_rt(NavState & nav_state) } if (has_local_filter_) { ->>>>>>> kilted ukf_local_->periodicUpdate(); nav_msgs::msg::Odometry local_odom; diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index b66fad90..f7f6eff2 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -2,11 +2,7 @@ easynav_gps_localizer -<<<<<<< HEAD - 0.2.1 -======= 0.3.1 ->>>>>>> kilted Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp index 6b9ea4a4..ffd56be4 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/PerceptionModel.hpp @@ -12,10 +12,6 @@ // 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. -<<<<<<< HEAD -======= - ->>>>>>> kilted #ifndef EASYNAV_NAVMAP_LOCALIZER__PERCEPTIONMODEL_HPP_ #define EASYNAV_NAVMAP_LOCALIZER__PERCEPTIONMODEL_HPP_ diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 40c0e121..72286a33 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -2,11 +2,7 @@ easynav_navmap_localizer -<<<<<<< HEAD - 0.2.1 -======= 0.3.1 ->>>>>>> kilted Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index db50cf6f..08526afb 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -2,11 +2,7 @@ easynav_simple_localizer -<<<<<<< HEAD - 0.2.1 -======= 0.3.1 ->>>>>>> kilted Easy Navigation: Simple Localizer package. Francisco Martín Rico Apache-2.0 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 b63f2c26..b5c674af 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -554,11 +554,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(tf_info.robot_frame) -======= .fuse(tf_info.robot_footprint_frame, last_input_time_) ->>>>>>> kilted .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.resolution()) diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index ec8d4e2e..12dd1636 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -108,101 +108,6 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG const auto infos = node->get_subscriptions_info_by_topic("initialpose"); ASSERT_FALSE(infos.empty()); -<<<<<<< HEAD - auto cell2 = map_ptr->metric_to_cell(-1.0, -1.0); - EXPECT_TRUE(map_ptr->at(cell2.first, cell2.second)); -} - -/// \brief Map loading via subscription test -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) -{ - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - easynav::TFInfo tf_info; - tf_info.map_frame = "world_map"; - tf_info.odom_frame = "world_odom"; - tf_info.robot_frame = "world_base"; - tf_info.robot_frame = "world_footprint_base"; - easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - rclcpp::Publisher::SharedPtr pub = - node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = tf_info.map_frame; - 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; // Mark cell (5,5) as occupied - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - - EXPECT_EQ(static_map->at(5, 5), 1); - EXPECT_EQ(static_map->at(1, 1), 0); -} - -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { -public: - void force_path(const std::string & path) {map_path_ = path;} -}; - -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) -{ - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared(); - static_map->initialize(4, 4, 0.5, -1.0, -1.0, 0); - static_map->at(1, 1) = true; - static_map->at(2, 2) = true; - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map.txt"; - const std::string service_name = "/test_savemap_node/test_savemap/savemap"; - - std::static_pointer_cast(manager)->force_path(test_map_file); - - 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))); - - 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); - - std::ifstream infile(test_map_file); - ASSERT_TRUE(infile.is_open()); - - std::string first_line; - std::getline(infile, first_line); - EXPECT_NE(first_line.find("4 4"), std::string::npos); - EXPECT_NE(first_line.find("0.5"), std::string::npos); - - infile.close(); - std::remove(test_map_file.c_str()); -======= const bool has_expected_type = std::any_of( infos.begin(), infos.end(), [](const rclcpp::TopicEndpointInfo & info) { @@ -273,5 +178,4 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks) EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); } ->>>>>>> kilted } 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 e872a3ec..2561667f 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 @@ -12,10 +12,6 @@ // 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. -<<<<<<< HEAD -======= - ->>>>>>> kilted #include 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 76007645..1ea4a111 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 @@ -12,10 +12,6 @@ // 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. -<<<<<<< HEAD -======= - ->>>>>>> kilted #include diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 41cccb82..0efe4f14 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -2,11 +2,7 @@ easynav_simple_maps_manager -<<<<<<< HEAD - 0.2.1 -======= 0.3.1 ->>>>>>> kilted Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 From 557fff7e8cd5ed8a16fcd9316e2a7bdf9f0e5a69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Jul 2026 12:01:26 +0200 Subject: [PATCH 217/230] Add missing easynav_sensors deps MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mpc_controller/CMakeLists.txt | 3 +++ controllers/easynav_mpc_controller/package.xml | 1 + controllers/easynav_mppi_controller/CMakeLists.txt | 3 +++ controllers/easynav_mppi_controller/package.xml | 1 + controllers/easynav_regulated_pp_controller/CMakeLists.txt | 3 +++ controllers/easynav_serest_controller/CMakeLists.txt | 3 +++ controllers/easynav_serest_controller/package.xml | 1 + controllers/easynav_vff_controller/CMakeLists.txt | 3 +++ controllers/easynav_vff_controller/package.xml | 1 + localizers/easynav_costmap_localizer/CMakeLists.txt | 3 +++ localizers/easynav_costmap_localizer/package.xml | 1 + localizers/easynav_fusion_localizer/CMakeLists.txt | 3 +++ localizers/easynav_fusion_localizer/package.xml | 1 + localizers/easynav_navmap_localizer/CMakeLists.txt | 3 +++ localizers/easynav_navmap_localizer/package.xml | 1 + localizers/easynav_simple_localizer/CMakeLists.txt | 3 +++ localizers/easynav_simple_localizer/package.xml | 1 + maps_managers/easynav_costmap_maps_manager/CMakeLists.txt | 3 +++ maps_managers/easynav_costmap_maps_manager/package.xml | 1 + maps_managers/easynav_navmap_maps_manager/CMakeLists.txt | 3 +++ maps_managers/easynav_navmap_maps_manager/package.xml | 1 + maps_managers/easynav_simple_maps_manager/CMakeLists.txt | 1 + maps_managers/easynav_simple_maps_manager/package.xml | 1 + maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt | 2 +- 24 files changed, 46 insertions(+), 1 deletion(-) diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index ef47e161..b80ddbb7 100644 --- a/controllers/easynav_mpc_controller/CMakeLists.txt +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_system REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -42,6 +43,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core easynav_system::easynav_system + easynav_sensors::easynav_sensors tf2_ros::tf2_ros pluginlib::pluginlib Eigen3::Eigen @@ -84,6 +86,7 @@ pluginlib_export_plugin_description_file(easynav_core easynav_mpc_controller_plu ament_export_dependencies( easynav_common easynav_core + easynav_sensors pluginlib tf2_ros geometry_msgs diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 038ff4ed..e0a04234 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -15,6 +15,7 @@ easynav_common easynav_core easynav_system + easynav_sensors pluginlib tf2_ros geometry_msgs diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index 4edc78f1..c0428cf5 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/controllers/easynav_mppi_controller/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_system REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -31,6 +32,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core easynav_system::easynav_system + easynav_sensors::easynav_sensors tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs pluginlib::pluginlib @@ -73,6 +75,7 @@ pluginlib_export_plugin_description_file(easynav_core easynav_mppi_controller_pl ament_export_dependencies( easynav_common easynav_core + easynav_sensors pluginlib tf2_ros tf2_geometry_msgs diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index ff401d2c..82bd48ac 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -12,6 +12,7 @@ easynav_common easynav_core easynav_system + easynav_sensors pluginlib tf2_ros tf2_geometry_msgs diff --git a/controllers/easynav_regulated_pp_controller/CMakeLists.txt b/controllers/easynav_regulated_pp_controller/CMakeLists.txt index c8757528..8f54c652 100644 --- a/controllers/easynav_regulated_pp_controller/CMakeLists.txt +++ b/controllers/easynav_regulated_pp_controller/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_system REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -29,6 +30,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core easynav_system::easynav_system + easynav_sensors::easynav_sensors tf2_ros::tf2_ros tf2::tf2 tf2_geometry_msgs::tf2_geometry_msgs @@ -72,6 +74,7 @@ ament_export_dependencies( easynav_common easynav_core easynav_system + easynav_sensors pluginlib tf2_ros tf2 diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt index 110edf69..147c8141 100644 --- a/controllers/easynav_serest_controller/CMakeLists.txt +++ b/controllers/easynav_serest_controller/CMakeLists.txt @@ -8,6 +8,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -26,6 +27,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + easynav_sensors::easynav_sensors tf2_ros::tf2_ros tf2::tf2 tf2_geometry_msgs::tf2_geometry_msgs @@ -66,6 +68,7 @@ pluginlib_export_plugin_description_file(easynav_core easynav_serest_controller_ ament_export_dependencies( easynav_common easynav_core + easynav_sensors pluginlib tf2_ros tf2 diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 83cfb24c..8fbf7f38 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -11,6 +11,7 @@ easynav_common easynav_core + easynav_sensors pluginlib tf2 tf2_geometry_msgs diff --git a/controllers/easynav_vff_controller/CMakeLists.txt b/controllers/easynav_vff_controller/CMakeLists.txt index 04a2da17..cbc0fbd8 100644 --- a/controllers/easynav_vff_controller/CMakeLists.txt +++ b/controllers/easynav_vff_controller/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_common REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(nav_msgs REQUIRED) find_package(PCL REQUIRED COMPONENTS common io) find_package(pluginlib REQUIRED) @@ -21,6 +22,7 @@ set(dependencies rclcpp_lifecycle easynav_core easynav_common + easynav_sensors pluginlib visualization_msgs tf2 @@ -37,6 +39,7 @@ target_include_directories(vff_controller PUBLIC target_link_libraries(vff_controller easynav_core::easynav_core easynav_common::easynav_common + easynav_sensors::easynav_sensors pluginlib::pluginlib ${nav_msgs_TARGETS} ${visualization_msgs_TARGETS} diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index 2de0dcf4..dab176a2 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -15,6 +15,7 @@ easynav_common nav_msgs easynav_core + easynav_sensors pcl_ros visualization_msgs diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index 15c2642b..477250e6 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(easynav_common REQUIRED) find_package(easynav_costmap_common REQUIRED) find_package(easynav_localizer REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -30,6 +31,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_costmap_common::easynav_costmap_common easynav_localizer::easynav_localizer + easynav_sensors::easynav_sensors tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib @@ -73,6 +75,7 @@ ament_export_dependencies( easynav_costmap_common easynav_localizer easynav_core + easynav_sensors tf2 tf2_ros pluginlib diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index bc8b00d0..1ead73ff 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -13,6 +13,7 @@ easynav_costmap_common easynav_localizer easynav_core + easynav_sensors pluginlib tf2 tf2_ros diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index c0755cd4..6a7f417c 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_localizer REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(geographic_msgs REQUIRED) find_package(robot_localization REQUIRED) @@ -30,6 +31,7 @@ set(dependencies easynav_core easynav_common easynav_localizer + easynav_sensors pluginlib geographic_msgs robot_localization @@ -49,6 +51,7 @@ target_link_libraries(fusion_localizer easynav_core::easynav_core easynav_common::easynav_common easynav_localizer::easynav_localizer + easynav_sensors::easynav_sensors pluginlib::pluginlib ${geographic_msgs_TARGETS} GeographicLib diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 8c9f678e..341fd956 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -15,6 +15,7 @@ easynav_core easynav_common easynav_localizer + easynav_sensors pluginlib geographiclib geographic_msgs diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index 8bd6781e..2db85f56 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(navmap_ros_interfaces REQUIRED) find_package(easynav_localizer REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_bonxai_maps_manager REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -36,6 +37,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_bonxai_maps_manager::easynav_bonxai_maps_manager ${navmap_ros_interfaces_TARGETS} easynav_localizer::easynav_localizer + easynav_sensors::easynav_sensors tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib @@ -82,6 +84,7 @@ ament_export_dependencies( easynav_localizer easynav_core easynav_bonxai_maps_manager + easynav_sensors tf2 tf2_ros pluginlib diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index 72286a33..a0879e5f 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -16,6 +16,7 @@ easynav_bonxai_maps_manager easynav_localizer easynav_core + easynav_sensors pluginlib tf2 tf2_ros diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index a26bb848..37e08da5 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(easynav_common REQUIRED) find_package(easynav_simple_common REQUIRED) find_package(easynav_localizer REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -30,6 +31,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_simple_common::easynav_simple_common easynav_localizer::easynav_localizer + easynav_sensors::easynav_sensors tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib @@ -73,6 +75,7 @@ ament_export_dependencies( easynav_simple_common easynav_localizer easynav_core + easynav_sensors tf2 tf2_ros pluginlib diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index 08526afb..780121e5 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -13,6 +13,7 @@ easynav_simple_common easynav_localizer easynav_core + easynav_sensors pluginlib tf2 tf2_ros diff --git a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt index 15c6c401..52dcc80e 100644 --- a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(easynav_costmap_common REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(std_srvs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaets REQUIRED) @@ -39,6 +40,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC pluginlib::pluginlib ament_index_cpp::ament_index_cpp easynav_costmap_common::easynav_costmap_common + easynav_sensors::easynav_sensors yaets::yaets ${nav_msgs_TARGETS} ${std_srvs_TARGETS} @@ -87,6 +89,7 @@ ament_export_dependencies( nav_msgs ament_index_cpp easynav_costmap_common + easynav_sensors std_srvs yaets yaml_cpp_vendor diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 1cdf1446..b48ed948 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -18,6 +18,7 @@ nav_msgs ament_index_cpp easynav_costmap_common + easynav_sensors std_srvs yaets yaml_cpp_vendor diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index ba12d985..ed1871ff 100644 --- a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(ament_index_cpp REQUIRED) find_package(navmap_core REQUIRED) find_package(navmap_ros REQUIRED) find_package(navmap_ros_interfaces REQUIRED) +find_package(easynav_sensors REQUIRED) find_package(std_srvs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) @@ -46,6 +47,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ament_index_cpp::ament_index_cpp navmap_core::navmap_core navmap_ros::navmap_ros + easynav_sensors::easynav_sensors ${sensor_msgs_TARGETS} ${tf2_geometry_msgs_TARGETS} ${navmap_ros_interfaces_TARGETS} @@ -100,6 +102,7 @@ ament_export_dependencies( navmap_core navmap_ros navmap_ros_interfaces + easynav_sensors std_srvs yaets yaml_cpp_vendor diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index c73f6f4e..5db092eb 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -21,6 +21,7 @@ navmap_core navmap_ros navmap_ros_interfaces + easynav_sensors std_srvs yaets yaml_cpp_vendor diff --git a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt index b47cd6cf..2c552332 100644 --- a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt @@ -55,6 +55,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + find_package(easynav_sensors REQUIRED) add_subdirectory(tests) endif() diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 0efe4f14..5582638d 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -24,6 +24,7 @@ ament_lint_auto ament_lint_common ament_cmake_gtest + easynav_sensors ament_cmake diff --git a/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt index 003f5ef5..4ced2ca8 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt @@ -1,2 +1,2 @@ ament_add_gtest(simple_mapsmanager_tests simple_mapsmanager_tests.cpp) -target_link_libraries(simple_mapsmanager_tests ${PROJECT_NAME}) +target_link_libraries(simple_mapsmanager_tests ${PROJECT_NAME} easynav_sensors::easynav_sensors) From 318cb4dbd7b01ee51c731bdd7545f247e7b04fc6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Jul 2026 12:15:56 +0200 Subject: [PATCH 218/230] Add missing easynav_sensors deps 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 | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index f12799f9..89695fac 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -25,6 +25,7 @@ jobs: with: package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { From 350a0fb2f3c47254211bba6362fbc97359ccb2e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Jul 2026 12:39:09 +0200 Subject: [PATCH 219/230] Add missing easynav_sensors deps MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- common/easynav_costmap_common/CMakeLists.txt | 6 + common/easynav_costmap_common/package.xml | 2 + .../easynav_mpc_controller/CMakeLists.txt | 9 + .../easynav_mpc_controller/package.xml | 4 + .../easynav_mppi_controller/CMakeLists.txt | 3 + .../easynav_mppi_controller/package.xml | 1 + .../easynav_serest_controller/CMakeLists.txt | 3 + .../easynav_serest_controller/package.xml | 1 + .../easynav_simple_controller/CMakeLists.txt | 3 + .../easynav_simple_controller/package.xml | 1 + .../easynav_vff_controller/CMakeLists.txt | 4 + .../easynav_vff_controller/package.xml | 1 + .../easynav_costmap_localizer/CMakeLists.txt | 3 + .../easynav_costmap_localizer/package.xml | 1 + .../easynav_fusion_localizer/CMakeLists.txt | 27 +++ .../easynav_fusion_localizer/package.xml | 9 + .../easynav_gps_localizer/CMakeLists.txt | 21 +++ localizers/easynav_gps_localizer/package.xml | 6 + .../easynav_navmap_localizer/CMakeLists.txt | 7 + .../easynav_navmap_localizer/package.xml | 2 + .../tests/costmap_localizer_tests.cpp | 157 ------------------ .../easynav_simple_localizer/CMakeLists.txt | 3 + .../easynav_simple_localizer/package.xml | 1 + .../CMakeLists.txt | 4 + .../easynav_bonxai_maps_manager/package.xml | 1 + .../CMakeLists.txt | 13 ++ .../easynav_costmap_maps_manager/package.xml | 6 +- .../CMakeLists.txt | 10 ++ .../easynav_navmap_maps_manager/package.xml | 3 + .../CMakeLists.txt | 7 + .../easynav_octomap_maps_manager/package.xml | 2 + .../CMakeLists.txt | 7 + .../easynav_routes_maps_manager/package.xml | 4 +- .../CMakeLists.txt | 3 + .../easynav_simple_maps_manager/package.xml | 2 + .../tests/CMakeLists.txt | 7 +- 36 files changed, 182 insertions(+), 162 deletions(-) delete mode 100644 localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp diff --git a/common/easynav_costmap_common/CMakeLists.txt b/common/easynav_costmap_common/CMakeLists.txt index 243de87a..de103729 100644 --- a/common/easynav_costmap_common/CMakeLists.txt +++ b/common/easynav_costmap_common/CMakeLists.txt @@ -13,6 +13,8 @@ find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -26,6 +28,8 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common ${nav_msgs_TARGETS} rclcpp::rclcpp + ${geometry_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs ) install( @@ -59,5 +63,7 @@ ament_export_dependencies( # pluginlib nav_msgs rclcpp + geometry_msgs + tf2_geometry_msgs ) ament_package() diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 2170fe49..f48c7c5a 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -23,6 +23,8 @@ nav_msgs rclcpp ament_index_cpp + geometry_msgs + tf2_geometry_msgs ament_lint_auto ament_lint_common diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index b80ddbb7..a8ff1d0b 100644 --- a/controllers/easynav_mpc_controller/CMakeLists.txt +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -13,9 +13,12 @@ find_package(easynav_sensors REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(Eigen3 REQUIRED NO_MODULE) find_package(PCL REQUIRED COMPONENTS common io) +find_package(pcl_conversions REQUIRED) find_package(NLopt QUIET) if(NOT NLopt_FOUND) find_path(NLOPT_INCLUDE_DIR nlopt.hpp) @@ -38,6 +41,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $ ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} ) target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common @@ -45,11 +49,13 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_system::easynav_system easynav_sensors::easynav_sensors tf2_ros::tf2_ros + tf2::tf2 pluginlib::pluginlib Eigen3::Eigen nlopt ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} ${PCL_LIBRARIES} ) @@ -89,8 +95,11 @@ ament_export_dependencies( easynav_sensors pluginlib tf2_ros + tf2 geometry_msgs nav_msgs + sensor_msgs + pcl_conversions eigen nlopt ) diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index e0a04234..e1c1f433 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -18,8 +18,12 @@ easynav_sensors pluginlib tf2_ros + tf2 geometry_msgs nav_msgs + sensor_msgs + pcl_conversions + eigen rclcpp_lifecycle easynav_simple_common diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index c0428cf5..19b6bcdd 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/controllers/easynav_mppi_controller/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(easynav_sensors 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) find_package(visualization_msgs REQUIRED) @@ -34,6 +35,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_system::easynav_system easynav_sensors::easynav_sensors tf2_ros::tf2_ros + tf2::tf2 tf2_geometry_msgs::tf2_geometry_msgs pluginlib::pluginlib ${geometry_msgs_TARGETS} @@ -78,6 +80,7 @@ ament_export_dependencies( easynav_sensors pluginlib tf2_ros + tf2 tf2_geometry_msgs geometry_msgs nav_msgs diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 82bd48ac..c43e9da9 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -15,6 +15,7 @@ easynav_sensors pluginlib tf2_ros + tf2 tf2_geometry_msgs geometry_msgs nav_msgs diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt index 147c8141..b0b5b0e4 100644 --- a/controllers/easynav_serest_controller/CMakeLists.txt +++ b/controllers/easynav_serest_controller/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_sensors REQUIRED) +find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -28,6 +29,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core easynav_sensors::easynav_sensors + rclcpp::rclcpp tf2_ros::tf2_ros tf2::tf2 tf2_geometry_msgs::tf2_geometry_msgs @@ -69,6 +71,7 @@ ament_export_dependencies( easynav_common easynav_core easynav_sensors + rclcpp pluginlib tf2_ros tf2 diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 8fbf7f38..222e4572 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -12,6 +12,7 @@ easynav_common easynav_core easynav_sensors + rclcpp pluginlib tf2 tf2_geometry_msgs diff --git a/controllers/easynav_simple_controller/CMakeLists.txt b/controllers/easynav_simple_controller/CMakeLists.txt index 4bd163a9..f99d627d 100644 --- a/controllers/easynav_simple_controller/CMakeLists.txt +++ b/controllers/easynav_simple_controller/CMakeLists.txt @@ -8,6 +8,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -27,6 +28,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp tf2_ros::tf2_ros tf2::tf2 tf2_geometry_msgs::tf2_geometry_msgs @@ -68,6 +70,7 @@ pluginlib_export_plugin_description_file(easynav_core easynav_simple_controller_ ament_export_dependencies( easynav_common easynav_core + rclcpp pluginlib tf2_ros tf2 diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 7bf9b269..852a51ef 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -11,6 +11,7 @@ easynav_common easynav_core + rclcpp pluginlib tf2 tf2_geometry_msgs diff --git a/controllers/easynav_vff_controller/CMakeLists.txt b/controllers/easynav_vff_controller/CMakeLists.txt index cbc0fbd8..fcb01550 100644 --- a/controllers/easynav_vff_controller/CMakeLists.txt +++ b/controllers/easynav_vff_controller/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(easynav_common REQUIRED) find_package(easynav_sensors REQUIRED) find_package(nav_msgs REQUIRED) find_package(PCL REQUIRED COMPONENTS common io) +find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(visualization_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -25,6 +26,8 @@ set(dependencies easynav_sensors pluginlib visualization_msgs + geometry_msgs + nav_msgs tf2 ) @@ -42,6 +45,7 @@ target_link_libraries(vff_controller easynav_sensors::easynav_sensors pluginlib::pluginlib ${nav_msgs_TARGETS} + ${geometry_msgs_TARGETS} ${visualization_msgs_TARGETS} ${PCL_LIBRARIES} ) diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index dab176a2..43d24b8c 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -17,6 +17,7 @@ easynav_core easynav_sensors pcl_ros + geometry_msgs visualization_msgs ament_lint_auto diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index 477250e6..65265795 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) add_library(${PROJECT_NAME} SHARED @@ -35,6 +36,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib + Eigen3::Eigen ${tf2_geometry_msgs_TARGETS} ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -82,5 +84,6 @@ ament_export_dependencies( geometry_msgs std_srvs nav_msgs + eigen ) ament_package() diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index 1ead73ff..b11b0001 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -20,6 +20,7 @@ tf2_geometry_msgs geometry_msgs nav_msgs + eigen rclcpp rclcpp_lifecycle diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index 6a7f417c..a2841325 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -17,6 +17,15 @@ find_package(geographic_msgs REQUIRED) find_package(robot_localization REQUIRED) find_package(std_srvs REQUIRED) find_package(angles REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) # Geographiclib installs FindGeographicLib.cmake to this non-standard location set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") @@ -35,6 +44,15 @@ set(dependencies pluginlib geographic_msgs robot_localization + diagnostic_msgs + diagnostic_updater + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + eigen ) add_library(fusion_localizer SHARED @@ -59,6 +77,15 @@ target_link_libraries(fusion_localizer ${robot_localization_LIBRARIES} angles::angles yaets::yaets + diagnostic_updater::diagnostic_updater + ${diagnostic_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + Eigen3::Eigen ) target_compile_definitions(fusion_localizer PUBLIC EASYNAV_DEBUG_WITH_YAETS) diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 341fd956..20cf9852 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -22,6 +22,15 @@ robot_localization std_srvs angles + diagnostic_msgs + diagnostic_updater + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + eigen ament_lint_auto ament_lint_common diff --git a/localizers/easynav_gps_localizer/CMakeLists.txt b/localizers/easynav_gps_localizer/CMakeLists.txt index b44c29b4..3341e765 100644 --- a/localizers/easynav_gps_localizer/CMakeLists.txt +++ b/localizers/easynav_gps_localizer/CMakeLists.txt @@ -9,8 +9,15 @@ 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(pluginlib REQUIRED) find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) # Geographiclib installs FindGeographicLib.cmake to this non-standard location set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") @@ -23,8 +30,15 @@ set(dependencies rclcpp rclcpp_lifecycle easynav_core + easynav_common pluginlib geographic_msgs + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros ) add_library(gps_localizer SHARED @@ -36,10 +50,17 @@ target_include_directories(gps_localizer PUBLIC ) target_link_libraries(gps_localizer easynav_core::easynav_core + easynav_common::easynav_common pluginlib::pluginlib ${geographic_msgs_TARGETS} GeographicLib ${GeographicLib_LIBRARIES} + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros ) install(DIRECTORY include/ diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index f7f6eff2..998e9c2c 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -17,6 +17,12 @@ pluginlib geographiclib geographic_msgs + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros ament_lint_auto ament_lint_common diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index 2db85f56..37056907 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/CMakeLists.txt @@ -21,6 +21,8 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(Eigen3 REQUIRED NO_MODULE) add_library(${PROJECT_NAME} SHARED @@ -29,6 +31,7 @@ add_library(${PROJECT_NAME} SHARED target_include_directories(${PROJECT_NAME} PUBLIC $ $ + ${PCL_INCLUDE_DIRS} ) target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common @@ -41,9 +44,11 @@ target_link_libraries(${PROJECT_NAME} PUBLIC tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib + Eigen3::Eigen ${tf2_geometry_msgs_TARGETS} ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} + ${PCL_LIBRARIES} ) install( @@ -91,5 +96,7 @@ ament_export_dependencies( geometry_msgs std_srvs nav_msgs + PCL + eigen ) ament_package() diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index a0879e5f..cfeaa49a 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -23,6 +23,8 @@ tf2_geometry_msgs geometry_msgs nav_msgs + pcl_ros + eigen rclcpp rclcpp_lifecycle diff --git a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp deleted file mode 100644 index 7e9166a0..00000000 --- a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// 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 - -#include "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_navmap_common/costmap_2d.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "std_srvs/srv/trigger.hpp" - -#include -#include - -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test -{ -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) -{ - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) -} - -TEST_F(AMCLLocalizerTest, 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()); - - 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)); - - auto static_map = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); -} - -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { -public: - void force_path(const std::string & path) {map_path_ = path;} -}; - -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) -{ - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); - - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - - 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); - - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); - - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); - - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); -} diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index 37e08da5..85189806 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) add_library(${PROJECT_NAME} SHARED @@ -35,6 +36,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib + Eigen3::Eigen ${tf2_geometry_msgs_TARGETS} ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -82,5 +84,6 @@ ament_export_dependencies( geometry_msgs std_srvs nav_msgs + eigen ) ament_package() diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index 780121e5..fa0d0e27 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -20,6 +20,7 @@ tf2_geometry_msgs geometry_msgs nav_msgs + eigen rclcpp rclcpp_lifecycle diff --git a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt index eea955ee..90476e36 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) find_package(PCL REQUIRED COMPONENTS common io filters) find_package(pcl_conversions REQUIRED) find_package(std_srvs REQUIRED) @@ -55,6 +57,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib @@ -105,6 +108,7 @@ pluginlib_export_plugin_description_file(easynav_core easynav_bonxai_maps_manage ament_export_dependencies( easynav_common easynav_core + rclcpp tf2 tf2_ros pluginlib diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 7ca25f29..7d1d5b66 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -11,6 +11,7 @@ easynav_common easynav_core + rclcpp tf2 tf2_ros tf2_geometry_msgs diff --git a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt index 52dcc80e..ac3b93bc 100644 --- a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt @@ -10,7 +10,12 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) find_package(pluginlib REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(easynav_costmap_common REQUIRED) @@ -36,7 +41,11 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle tf2_ros::tf2_ros + tf2::tf2 + Eigen3::Eigen pluginlib::pluginlib ament_index_cpp::ament_index_cpp easynav_costmap_common::easynav_costmap_common @@ -84,7 +93,11 @@ pluginlib_export_plugin_description_file(easynav_costmap_maps_manager easynav_co ament_export_dependencies( easynav_common easynav_core + rclcpp + rclcpp_lifecycle tf2_ros + tf2 + eigen pluginlib nav_msgs ament_index_cpp diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index b48ed948..c4c15397 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -9,11 +9,13 @@ ament_cmake - + rclcpp + rclcpp_lifecycle easynav_common easynav_core tf2_ros + tf2 + eigen pluginlib nav_msgs ament_index_cpp diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index ed1871ff..42b24953 100644 --- a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt @@ -10,6 +10,10 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -41,6 +45,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + Eigen3::Eigen tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib @@ -94,6 +101,9 @@ pluginlib_export_plugin_description_file(easynav_navmap_maps_manager easynav_nav ament_export_dependencies( easynav_common easynav_core + rclcpp + rclcpp_lifecycle + eigen tf2 tf2_ros pluginlib diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index 5db092eb..6ad6a5ba 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -11,6 +11,9 @@ easynav_common easynav_core + rclcpp + rclcpp_lifecycle + eigen tf2 tf2_ros tf2_geometry_msgs diff --git a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt index fc7b4b24..f59f8814 100644 --- a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt @@ -10,6 +10,9 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -43,6 +46,8 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle tf2::tf2 tf2_ros::tf2_ros pluginlib::pluginlib @@ -92,6 +97,8 @@ pluginlib_export_plugin_description_file(easynav_core easynav_octomap_maps_manag ament_export_dependencies( easynav_common easynav_core + rclcpp + rclcpp_lifecycle tf2 tf2_ros pluginlib diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index e7fe9441..ef5297d6 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -11,6 +11,8 @@ easynav_common easynav_core + rclcpp + rclcpp_lifecycle tf2 tf2_ros tf2_geometry_msgs diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt index 18cd1a91..ca6c4211 100644 --- a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt @@ -9,6 +9,9 @@ find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) find_package(easynav_costmap_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) find_package(ament_index_cpp REQUIRED) @@ -32,6 +35,8 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core easynav_costmap_common::easynav_costmap_common + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle tf2_ros::tf2_ros pluginlib::pluginlib ament_index_cpp::ament_index_cpp @@ -77,6 +82,8 @@ pluginlib_export_plugin_description_file(easynav_routes_maps_manager easynav_rou ament_export_dependencies( easynav_common easynav_core + rclcpp + rclcpp_lifecycle tf2_ros pluginlib easynav_costmap_common diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index 0b9c9a6e..f73a0e66 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -9,8 +9,8 @@ ament_cmake - + rclcpp + rclcpp_lifecycle easynav_common easynav_core easynav_costmap_common diff --git a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt index 2c552332..6913465f 100644 --- a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(easynav_core REQUIRED) find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(nav_msgs REQUIRED) find_package(easynav_simple_common REQUIRED) find_package(std_srvs REQUIRED) find_package(yaets REQUIRED) @@ -56,6 +57,8 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(easynav_sensors REQUIRED) + find_package(rclcpp REQUIRED) + find_package(rclcpp_lifecycle REQUIRED) add_subdirectory(tests) endif() diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index 5582638d..da483e2a 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -25,6 +25,8 @@ ament_lint_common ament_cmake_gtest easynav_sensors + rclcpp + rclcpp_lifecycle ament_cmake diff --git a/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt index 4ced2ca8..6f74618f 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt @@ -1,2 +1,7 @@ ament_add_gtest(simple_mapsmanager_tests simple_mapsmanager_tests.cpp) -target_link_libraries(simple_mapsmanager_tests ${PROJECT_NAME} easynav_sensors::easynav_sensors) +target_link_libraries(simple_mapsmanager_tests + ${PROJECT_NAME} + easynav_sensors::easynav_sensors + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) From b2fa18617b84bce530958114fdddaca34b5c2589 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 26 Jul 2026 10:41:54 +0200 Subject: [PATCH 220/230] Update Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- common/easynav_costmap_common/CHANGELOG.rst | 8 +++++ common/easynav_simple_common/CHANGELOG.rst | 6 ++++ .../easynav_mpc_controller/CHANGELOG.rst | 34 +++++++++++++++++++ .../easynav_mppi_controller/CHANGELOG.rst | 7 ++++ .../CHANGELOG.rst | 20 +++++++++++ .../easynav_serest_controller/CHANGELOG.rst | 11 ++++++ .../easynav_simple_controller/CHANGELOG.rst | 7 ++++ .../easynav_vff_controller/CHANGELOG.rst | 9 +++++ .../easynav_costmap_localizer/CHANGELOG.rst | 15 ++++++++ .../easynav_fusion_localizer/CHANGELOG.rst | 25 +++++++++++++- .../easynav_gps_localizer/CHANGELOG.rst | 6 ++++ .../easynav_navmap_localizer/CHANGELOG.rst | 10 ++++++ .../easynav_simple_localizer/CHANGELOG.rst | 15 ++++++++ .../easynav_bonxai_maps_manager/CHANGELOG.rst | 7 ++++ .../CHANGELOG.rst | 19 +++++++++++ .../easynav_navmap_maps_manager/CHANGELOG.rst | 12 +++++++ .../CHANGELOG.rst | 9 +++++ .../easynav_routes_maps_manager/CHANGELOG.rst | 12 +++++++ .../easynav_simple_maps_manager/CHANGELOG.rst | 12 +++++++ .../easynav_costmap_planner/CHANGELOG.rst | 10 ++++++ planners/easynav_navmap_planner/CHANGELOG.rst | 6 ++++ planners/easynav_simple_planner/CHANGELOG.rst | 9 +++++ 22 files changed, 268 insertions(+), 1 deletion(-) create mode 100644 controllers/easynav_regulated_pp_controller/CHANGELOG.rst diff --git a/common/easynav_costmap_common/CHANGELOG.rst b/common/easynav_costmap_common/CHANGELOG.rst index 4cb20109..ab249852 100644 --- a/common/easynav_costmap_common/CHANGELOG.rst +++ b/common/easynav_costmap_common/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package easynav_costmap_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Any change in the costmap set it as modified +* Adding timestamp to costmap +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/common/easynav_simple_common/CHANGELOG.rst b/common/easynav_simple_common/CHANGELOG.rst index dda3a91f..3668ee77 100644 --- a/common/easynav_simple_common/CHANGELOG.rst +++ b/common/easynav_simple_common/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package easynav_simple_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Sync with Rolling branch +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst index a8a04271..5d85b834 100644 --- a/controllers/easynav_mpc_controller/CHANGELOG.rst +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package easynav_mpc_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + +* Add missing easynav_sensors deps +* Add missing easynav_sensors deps +* Sync with Rolling branch +* Sync with rolling branch +* Merge pull request `#61 `_ from Butakus/rolling + Update plugins to new sensors API +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* 0.3.1 +* update changelogs +* Merge branch 'rolling' of github.com:midemig/easynav_plugins into rolling +* Merge branch 'EasyNavigation:rolling' into rolling +* Merge pull request `#51 `_ from EasyNavigation/license-kilted + GPLv3 -> Apache 2.0 +* GPLv3 -> Apache 2.0 +* Merge pull request `#50 `_ from EasyNavigation/license + GPLv3 -> Apache 2.0 +* Merge pull request `#49 `_ from juanscelyg/rolling + Latest Updates and Corrected CI +* Merge pull request `#48 `_ from juanscelyg/kilted + Kilted +* GPLv3 -> Apache 2.0 +* Latest changes were added +* Dependencies were corrected +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.3.1 (2026-02-27) ------------------ * GPLv3 -> Apache 2.0 diff --git a/controllers/easynav_mppi_controller/CHANGELOG.rst b/controllers/easynav_mppi_controller/CHANGELOG.rst index 14ca76f7..2b4039de 100644 --- a/controllers/easynav_mppi_controller/CHANGELOG.rst +++ b/controllers/easynav_mppi_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package easynav_mppi_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/controllers/easynav_regulated_pp_controller/CHANGELOG.rst b/controllers/easynav_regulated_pp_controller/CHANGELOG.rst new file mode 100644 index 00000000..69dc5fe3 --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/CHANGELOG.rst @@ -0,0 +1,20 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_regulated_pp_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add missing easynav_sensors deps +* Fix overshoot when DWPP is activated +* Fix license and copyright +* Port of Regulated Pure Pursuit. Initial commit +* Contributors: Francisco Martín Rico + +0.3.1 (2026-02-27 17:53) +------------------------ + +0.2.1 (2026-02-27 14:48) +------------------------ + +0.0.2 (2025-10-12) +------------------ diff --git a/controllers/easynav_serest_controller/CHANGELOG.rst b/controllers/easynav_serest_controller/CHANGELOG.rst index 797b0426..1e5a973f 100644 --- a/controllers/easynav_serest_controller/CHANGELOG.rst +++ b/controllers/easynav_serest_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package easynav_serest_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Navstate key among filters are always map, not an arbitrary key +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* Fix segfault in some cases and reduce extrapolation to the future +* GPLv3 -> Apache 2.0 +* Sync to current EasyNavigation +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/controllers/easynav_simple_controller/CHANGELOG.rst b/controllers/easynav_simple_controller/CHANGELOG.rst index 7d3c92ef..4e8066d6 100644 --- a/controllers/easynav_simple_controller/CHANGELOG.rst +++ b/controllers/easynav_simple_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package easynav_simple_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Fix segfault in some cases and reduce extrapolation to the future +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/controllers/easynav_vff_controller/CHANGELOG.rst b/controllers/easynav_vff_controller/CHANGELOG.rst index d6f15f0e..b2c9aa4b 100644 --- a/controllers/easynav_vff_controller/CHANGELOG.rst +++ b/controllers/easynav_vff_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package easynav_vff_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Sync to current EasyNavigation +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/localizers/easynav_costmap_localizer/CHANGELOG.rst b/localizers/easynav_costmap_localizer/CHANGELOG.rst index b5c759e8..a45932cc 100644 --- a/localizers/easynav_costmap_localizer/CHANGELOG.rst +++ b/localizers/easynav_costmap_localizer/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_costmap_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Avoid creating objects every loop cycle +* Fix std::normal_distribution when stdev is zero +* Check if std is non-positive before creating std::normal_distribution + Dynamic base costmap +* Change map static to base +* Reset pose from RViz2 for every localizer +* Adaptations to `#94 `_ +* Merge branch 'EasyNavigation:rolling' into rolling +* GPLv3 -> Apache 2.0 +* Sync to current EasyNavigation +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/localizers/easynav_fusion_localizer/CHANGELOG.rst b/localizers/easynav_fusion_localizer/CHANGELOG.rst index 60cdb0e5..fecbca69 100644 --- a/localizers/easynav_fusion_localizer/CHANGELOG.rst +++ b/localizers/easynav_fusion_localizer/CHANGELOG.rst @@ -2,7 +2,30 @@ Changelog for package easynav_fusion_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.0.1 (2025-11-23) +Forthcoming +----------- +* Add missing easynav_sensors deps +* deleted tests related to initial pose param which was removed. Removed spaces +* fix: add gnss group to gps config and ge_group has group to meet new easynav features. Initial pose now changes initial utm pose to change robot position in the map +* Update README.md for all localizers +* Reset pose from RViz2 for every localizer +* Adaptations to `#94 `_ +* Update plugins to new sensors API + New version of fusion localizer tested in real robot and simulator +* Added config file for gazebo simulation demo +* Only starts filter if params are found +* params updated +* Added simple configuration to launch full demo +* Initialization considers IMU orientation +* Merge branch 'rolling' of github.com:midemig/easynav_plugins into rolling +* Call set pose srvc for first pose for fastest convergence. Updated params to stabilice the filter in summit +* GPLv3 -> Apache 2.0 +* fixed odom topic +* Several bugs fixed. Now it correctly integrates GPS measurements to the dual filter. +* Added dual ukf for local and global localization +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, midemig, migueldm + +0.0.2 (2025-11-23) ------------------ * First version * Contributors: Miguel Ángel de Miguel diff --git a/localizers/easynav_gps_localizer/CHANGELOG.rst b/localizers/easynav_gps_localizer/CHANGELOG.rst index 1320050f..6dacbf87 100644 --- a/localizers/easynav_gps_localizer/CHANGELOG.rst +++ b/localizers/easynav_gps_localizer/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package easynav_gps_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/localizers/easynav_navmap_localizer/CHANGELOG.rst b/localizers/easynav_navmap_localizer/CHANGELOG.rst index 37dca43e..4b8774cf 100644 --- a/localizers/easynav_navmap_localizer/CHANGELOG.rst +++ b/localizers/easynav_navmap_localizer/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package easynav_navmap_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Check if std is non-positive before creating std::normal_distribution +* Update README.md for all localizers +* Reset pose from RViz2 for every localizer +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.0.2 (2025-10-12) ------------------ * Reorganization initial diff --git a/localizers/easynav_simple_localizer/CHANGELOG.rst b/localizers/easynav_simple_localizer/CHANGELOG.rst index cf2a976b..f17020b5 100644 --- a/localizers/easynav_simple_localizer/CHANGELOG.rst +++ b/localizers/easynav_simple_localizer/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package easynav_simple_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Avoid creating objects every loop cycle +* Fix std::normal_distribution when stdev is zero +* Navstate key among filters are always map, not an arbitrary key +* Check if std is non-positive before creating std::normal_distribution +* Navstate key among filters are always map, not an arbitrary key +* Update README.md for all localizers +* Reset pose from RViz2 for every localizer +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst index af483566..433e750d 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package easynav_bonxai_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Update calls to deprecated get_package_share_directory +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst index 28309613..68210386 100644 --- a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package easynav_costmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Update dynamic_map\_ with map key at navstate +* Avoid rewrite map with dynamic_map +* Navstate key among filters are always map, not an arbitrary key +* Update calls to deprecated get_package_share_directory +* Update calls to deprecated get_package_share_directory +* Update dynamic_map\_ with map key at navstate +* Navstate key among filters are always map, not an arbitrary key +* Any change in the costmap set it as modified +* Publish changes in base map and be ready to save updated map +* Costamp base map sincronized with nav_state +* Change map static to base +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst index feedad33..419b423c 100644 --- a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package easynav_navmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Navstate key among filters are always map, not an arbitrary key +* Update calls to deprecated get_package_share_directory +* Update calls to deprecated get_package_share_directory +* Navstate key among filters are always map, not an arbitrary key +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst index d52660a8..53fbda96 100644 --- a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package easynav_octomap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Update calls to deprecated get_package_share_directory +* Adaptations to `#94 `_ +* Update plugins to new sensors API +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst index 58f9c205..60709a31 100644 --- a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Navstate key among filters are always map, not an arbitrary key +* Update calls to deprecated get_package_share_directory +* Update calls to deprecated get_package_share_directory +* Navstate key among filters are always map, not an arbitrary key +* Merge pull request `#57 `_ from midemig/savemap_default + Added default path for route if not specified +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst index 9fa91ba3..f6e0405d 100644 --- a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing easynav_sensors deps +* Navstate key among filters are always map, not an arbitrary key +* Update calls to deprecated get_package_share_directory +* Navstate key among filters are always map, not an arbitrary key +* Reset pose from RViz2 for every localizer +* Remove group points in tests +* Adaptations to `#94 `_ +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/planners/easynav_costmap_planner/CHANGELOG.rst b/planners/easynav_costmap_planner/CHANGELOG.rst index f1d5e9af..cbcd0076 100644 --- a/planners/easynav_costmap_planner/CHANGELOG.rst +++ b/planners/easynav_costmap_planner/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package easynav_costmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Sync with Rolling branch +* Cells cost have no real impact in path creation +* Navstate key among filters are always map, not an arbitrary key +* Cells cost have no real impact in path creation +* Fix segfault in some cases and reduce extrapolation to the future +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/planners/easynav_navmap_planner/CHANGELOG.rst b/planners/easynav_navmap_planner/CHANGELOG.rst index e2f86b4f..5002202f 100644 --- a/planners/easynav_navmap_planner/CHANGELOG.rst +++ b/planners/easynav_navmap_planner/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package easynav_navmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Sync with Rolling branch +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 diff --git a/planners/easynav_simple_planner/CHANGELOG.rst b/planners/easynav_simple_planner/CHANGELOG.rst index fe6ca827..dcfd1666 100644 --- a/planners/easynav_simple_planner/CHANGELOG.rst +++ b/planners/easynav_simple_planner/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package easynav_simple_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Sync with Rolling branch +* Navstate key among filters are always map, not an arbitrary key +* Navstate key among filters are always map, not an arbitrary key +* Fix segfault in some cases and reduce extrapolation to the future +* GPLv3 -> Apache 2.0 +* Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm + 0.2.1 (2026-02-27) ------------------ * 0.2.0 From 7fdd7f82c0fd44b08d0f233de7203615a401c93a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 26 Jul 2026 10:43:07 +0200 Subject: [PATCH 221/230] Update Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_regulated_pp_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controllers/easynav_regulated_pp_controller/package.xml b/controllers/easynav_regulated_pp_controller/package.xml index c1609bc4..2cc0c16a 100644 --- a/controllers/easynav_regulated_pp_controller/package.xml +++ b/controllers/easynav_regulated_pp_controller/package.xml @@ -2,7 +2,7 @@ easynav_regulated_pp_controller - 0.0.1 + 0.3.1 Easy Navigation: port of the Nav2 Regulated Pure Pursuit Controller. Francisco Martín Rico Apache-2.0 From 17ad790a3db8f633406395278c171422958480e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 26 Jul 2026 10:43:15 +0200 Subject: [PATCH 222/230] 0.4.0 --- common/easynav_costmap_common/CHANGELOG.rst | 4 ++-- common/easynav_costmap_common/package.xml | 2 +- common/easynav_simple_common/CHANGELOG.rst | 4 ++-- common/easynav_simple_common/package.xml | 2 +- controllers/easynav_mpc_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_mpc_controller/package.xml | 2 +- controllers/easynav_mppi_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_mppi_controller/package.xml | 2 +- controllers/easynav_regulated_pp_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_regulated_pp_controller/package.xml | 2 +- controllers/easynav_serest_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_serest_controller/package.xml | 2 +- controllers/easynav_simple_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_simple_controller/package.xml | 2 +- controllers/easynav_vff_controller/CHANGELOG.rst | 4 ++-- controllers/easynav_vff_controller/package.xml | 2 +- localizers/easynav_costmap_localizer/CHANGELOG.rst | 4 ++-- localizers/easynav_costmap_localizer/package.xml | 2 +- localizers/easynav_fusion_localizer/CHANGELOG.rst | 4 ++-- localizers/easynav_fusion_localizer/package.xml | 2 +- localizers/easynav_gps_localizer/CHANGELOG.rst | 4 ++-- localizers/easynav_gps_localizer/package.xml | 2 +- localizers/easynav_navmap_localizer/CHANGELOG.rst | 4 ++-- localizers/easynav_navmap_localizer/package.xml | 2 +- localizers/easynav_simple_localizer/CHANGELOG.rst | 4 ++-- localizers/easynav_simple_localizer/package.xml | 2 +- maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_bonxai_maps_manager/package.xml | 2 +- maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_costmap_maps_manager/package.xml | 2 +- maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_navmap_maps_manager/package.xml | 2 +- maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_octomap_maps_manager/package.xml | 2 +- maps_managers/easynav_routes_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_routes_maps_manager/package.xml | 2 +- maps_managers/easynav_simple_maps_manager/CHANGELOG.rst | 4 ++-- maps_managers/easynav_simple_maps_manager/package.xml | 2 +- planners/easynav_costmap_planner/CHANGELOG.rst | 4 ++-- planners/easynav_costmap_planner/package.xml | 2 +- planners/easynav_navmap_planner/CHANGELOG.rst | 4 ++-- planners/easynav_navmap_planner/package.xml | 2 +- planners/easynav_simple_planner/CHANGELOG.rst | 4 ++-- planners/easynav_simple_planner/package.xml | 2 +- 44 files changed, 66 insertions(+), 66 deletions(-) diff --git a/common/easynav_costmap_common/CHANGELOG.rst b/common/easynav_costmap_common/CHANGELOG.rst index ab249852..5b9c3369 100644 --- a/common/easynav_costmap_common/CHANGELOG.rst +++ b/common/easynav_costmap_common/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_costmap_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Any change in the costmap set it as modified * Adding timestamp to costmap diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index f48c7c5a..cf01089e 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -2,7 +2,7 @@ easynav_costmap_common - 0.3.1 + 0.4.0 Easy Navigation: Simple Common package. Francisco Martín Rico diff --git a/common/easynav_simple_common/CHANGELOG.rst b/common/easynav_simple_common/CHANGELOG.rst index 3668ee77..715de44e 100644 --- a/common/easynav_simple_common/CHANGELOG.rst +++ b/common/easynav_simple_common/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Sync with Rolling branch * GPLv3 -> Apache 2.0 * Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm diff --git a/common/easynav_simple_common/package.xml b/common/easynav_simple_common/package.xml index c02be4df..0838a159 100644 --- a/common/easynav_simple_common/package.xml +++ b/common/easynav_simple_common/package.xml @@ -2,7 +2,7 @@ easynav_simple_common - 0.3.1 + 0.4.0 Easy Navigation: Simple Common package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_mpc_controller/CHANGELOG.rst b/controllers/easynav_mpc_controller/CHANGELOG.rst index 5d85b834..b77f4bbc 100644 --- a/controllers/easynav_mpc_controller/CHANGELOG.rst +++ b/controllers/easynav_mpc_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_mpc_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Adaptations to `#94 `_ * Update plugins to new sensors API diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index e1c1f433..0a102524 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -2,7 +2,7 @@ easynav_mpc_controller - 0.3.1 + 0.4.0 Easy Navigation: MPC Controller package. Juan S. Cely G. Apache-2.0 diff --git a/controllers/easynav_mppi_controller/CHANGELOG.rst b/controllers/easynav_mppi_controller/CHANGELOG.rst index 2b4039de..dd3b7b6d 100644 --- a/controllers/easynav_mppi_controller/CHANGELOG.rst +++ b/controllers/easynav_mppi_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_mppi_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Update plugins to new sensors API * GPLv3 -> Apache 2.0 diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index c43e9da9..de1542b9 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -2,7 +2,7 @@ easynav_mppi_controller - 0.3.1 + 0.4.0 Easy Navigation: MPPI Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/controllers/easynav_regulated_pp_controller/CHANGELOG.rst b/controllers/easynav_regulated_pp_controller/CHANGELOG.rst index 69dc5fe3..1b79b819 100644 --- a/controllers/easynav_regulated_pp_controller/CHANGELOG.rst +++ b/controllers/easynav_regulated_pp_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_regulated_pp_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Fix overshoot when DWPP is activated * Fix license and copyright diff --git a/controllers/easynav_regulated_pp_controller/package.xml b/controllers/easynav_regulated_pp_controller/package.xml index 2cc0c16a..7aa5f5f9 100644 --- a/controllers/easynav_regulated_pp_controller/package.xml +++ b/controllers/easynav_regulated_pp_controller/package.xml @@ -2,7 +2,7 @@ easynav_regulated_pp_controller - 0.3.1 + 0.4.0 Easy Navigation: port of the Nav2 Regulated Pure Pursuit Controller. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_serest_controller/CHANGELOG.rst b/controllers/easynav_serest_controller/CHANGELOG.rst index 1e5a973f..d57cbc21 100644 --- a/controllers/easynav_serest_controller/CHANGELOG.rst +++ b/controllers/easynav_serest_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_serest_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Navstate key among filters are always map, not an arbitrary key * Adaptations to `#94 `_ diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 222e4572..cd44e27d 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -2,7 +2,7 @@ easynav_serest_controller - 0.3.1 + 0.4.0 Easy Navigation: SeReST Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_simple_controller/CHANGELOG.rst b/controllers/easynav_simple_controller/CHANGELOG.rst index 4e8066d6..8b9a48ea 100644 --- a/controllers/easynav_simple_controller/CHANGELOG.rst +++ b/controllers/easynav_simple_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Fix segfault in some cases and reduce extrapolation to the future * GPLv3 -> Apache 2.0 diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 852a51ef..16cda26a 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -2,7 +2,7 @@ easynav_simple_controller - 0.3.1 + 0.4.0 Easy Navigation: Simple Controller package. Francisco Martín Rico Apache-2.0 diff --git a/controllers/easynav_vff_controller/CHANGELOG.rst b/controllers/easynav_vff_controller/CHANGELOG.rst index b2c9aa4b..5c00f0ce 100644 --- a/controllers/easynav_vff_controller/CHANGELOG.rst +++ b/controllers/easynav_vff_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_vff_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Adaptations to `#94 `_ * Update plugins to new sensors API diff --git a/controllers/easynav_vff_controller/package.xml b/controllers/easynav_vff_controller/package.xml index 43d24b8c..f4e84caa 100644 --- a/controllers/easynav_vff_controller/package.xml +++ b/controllers/easynav_vff_controller/package.xml @@ -2,7 +2,7 @@ easynav_vff_controller - 0.3.1 + 0.4.0 Easy Navigation: VFF Controller package. Jose Miguel Guerrero Hernandez Apache-2.0 diff --git a/localizers/easynav_costmap_localizer/CHANGELOG.rst b/localizers/easynav_costmap_localizer/CHANGELOG.rst index a45932cc..92be0a26 100644 --- a/localizers/easynav_costmap_localizer/CHANGELOG.rst +++ b/localizers/easynav_costmap_localizer/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_costmap_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Avoid creating objects every loop cycle * Fix std::normal_distribution when stdev is zero diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index b11b0001..e97e0574 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_costmap_localizer - 0.3.1 + 0.4.0 Easy Navigation: Costmap Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_fusion_localizer/CHANGELOG.rst b/localizers/easynav_fusion_localizer/CHANGELOG.rst index fecbca69..a00bb215 100644 --- a/localizers/easynav_fusion_localizer/CHANGELOG.rst +++ b/localizers/easynav_fusion_localizer/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_fusion_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * deleted tests related to initial pose param which was removed. Removed spaces * fix: add gnss group to gps config and ge_group has group to meet new easynav features. Initial pose now changes initial utm pose to change robot position in the map diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 20cf9852..4c698bcf 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -2,7 +2,7 @@ easynav_fusion_localizer - 0.3.1 + 0.4.0 Easy Navigation: Fusion Localizer package. Miguel Ángel de Miguel diff --git a/localizers/easynav_gps_localizer/CHANGELOG.rst b/localizers/easynav_gps_localizer/CHANGELOG.rst index 6dacbf87..4f7d44e9 100644 --- a/localizers/easynav_gps_localizer/CHANGELOG.rst +++ b/localizers/easynav_gps_localizer/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_gps_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * GPLv3 -> Apache 2.0 * Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml index 998e9c2c..00de8bf8 100644 --- a/localizers/easynav_gps_localizer/package.xml +++ b/localizers/easynav_gps_localizer/package.xml @@ -2,7 +2,7 @@ easynav_gps_localizer - 0.3.1 + 0.4.0 Easy Navigation: GPS Localizer package. Jose Miguel Guerrero Hernandez diff --git a/localizers/easynav_navmap_localizer/CHANGELOG.rst b/localizers/easynav_navmap_localizer/CHANGELOG.rst index 4b8774cf..2b25c821 100644 --- a/localizers/easynav_navmap_localizer/CHANGELOG.rst +++ b/localizers/easynav_navmap_localizer/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_navmap_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Check if std is non-positive before creating std::normal_distribution * Update README.md for all localizers diff --git a/localizers/easynav_navmap_localizer/package.xml b/localizers/easynav_navmap_localizer/package.xml index cfeaa49a..3c0e38e8 100644 --- a/localizers/easynav_navmap_localizer/package.xml +++ b/localizers/easynav_navmap_localizer/package.xml @@ -2,7 +2,7 @@ easynav_navmap_localizer - 0.3.1 + 0.4.0 Easy Navigation: nAVmAP Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/localizers/easynav_simple_localizer/CHANGELOG.rst b/localizers/easynav_simple_localizer/CHANGELOG.rst index f17020b5..d1595d98 100644 --- a/localizers/easynav_simple_localizer/CHANGELOG.rst +++ b/localizers/easynav_simple_localizer/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Avoid creating objects every loop cycle * Fix std::normal_distribution when stdev is zero diff --git a/localizers/easynav_simple_localizer/package.xml b/localizers/easynav_simple_localizer/package.xml index fa0d0e27..8936b16d 100644 --- a/localizers/easynav_simple_localizer/package.xml +++ b/localizers/easynav_simple_localizer/package.xml @@ -2,7 +2,7 @@ easynav_simple_localizer - 0.3.1 + 0.4.0 Easy Navigation: Simple Localizer package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst index 433e750d..e7537fc2 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_bonxai_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_bonxai_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Update calls to deprecated get_package_share_directory * GPLv3 -> Apache 2.0 diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 7d1d5b66..d1abc1da 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_bonxai_maps_manager - 0.3.1 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst index 68210386..e913dfbf 100644 --- a/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_costmap_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_costmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Update dynamic_map\_ with map key at navstate * Avoid rewrite map with dynamic_map diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index c4c15397..b07fe435 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_costmap_maps_manager - 0.3.1 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst index 419b423c..3a38c20b 100644 --- a/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_navmap_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_navmap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Navstate key among filters are always map, not an arbitrary key * Update calls to deprecated get_package_share_directory diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index 6ad6a5ba..570a046c 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_navmap_maps_manager - 0.3.1 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst index 53fbda96..ef361075 100644 --- a/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_octomap_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_octomap_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Update calls to deprecated get_package_share_directory * Adaptations to `#94 `_ diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index ef5297d6..08ebd1c5 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_octomap_maps_manager - 0.3.1 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst index 60709a31..4c78f245 100644 --- a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Navstate key among filters are always map, not an arbitrary key * Update calls to deprecated get_package_share_directory diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index f73a0e66..be6c1b61 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_routes_maps_manager - 0.3.1 + 0.4.0 Easy Navigation: Routes MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst index f6e0405d..7e5d07a8 100644 --- a/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst +++ b/maps_managers/easynav_simple_maps_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_maps_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Add missing easynav_sensors deps * Navstate key among filters are always map, not an arbitrary key * Update calls to deprecated get_package_share_directory diff --git a/maps_managers/easynav_simple_maps_manager/package.xml b/maps_managers/easynav_simple_maps_manager/package.xml index da483e2a..8c0c2c2f 100644 --- a/maps_managers/easynav_simple_maps_manager/package.xml +++ b/maps_managers/easynav_simple_maps_manager/package.xml @@ -2,7 +2,7 @@ easynav_simple_maps_manager - 0.3.1 + 0.4.0 Easy Navigation: Simple MapsManager package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_costmap_planner/CHANGELOG.rst b/planners/easynav_costmap_planner/CHANGELOG.rst index cbcd0076..f3cdd8f3 100644 --- a/planners/easynav_costmap_planner/CHANGELOG.rst +++ b/planners/easynav_costmap_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_costmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Sync with Rolling branch * Cells cost have no real impact in path creation * Navstate key among filters are always map, not an arbitrary key diff --git a/planners/easynav_costmap_planner/package.xml b/planners/easynav_costmap_planner/package.xml index 8471516d..d309dc59 100644 --- a/planners/easynav_costmap_planner/package.xml +++ b/planners/easynav_costmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_costmap_planner - 0.3.1 + 0.4.0 Easy Navigation: Costmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_navmap_planner/CHANGELOG.rst b/planners/easynav_navmap_planner/CHANGELOG.rst index 5002202f..68008d52 100644 --- a/planners/easynav_navmap_planner/CHANGELOG.rst +++ b/planners/easynav_navmap_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_navmap_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Sync with Rolling branch * GPLv3 -> Apache 2.0 * Contributors: Francisco Martín Rico, Juan S. Cely G., Miguel, migueldm diff --git a/planners/easynav_navmap_planner/package.xml b/planners/easynav_navmap_planner/package.xml index 7c297e25..872dc366 100644 --- a/planners/easynav_navmap_planner/package.xml +++ b/planners/easynav_navmap_planner/package.xml @@ -2,7 +2,7 @@ easynav_navmap_planner - 0.3.1 + 0.4.0 Easy Navigation: navmap planner package. Francisco Martín Rico Apache-2.0 diff --git a/planners/easynav_simple_planner/CHANGELOG.rst b/planners/easynav_simple_planner/CHANGELOG.rst index dcfd1666..8fa451f8 100644 --- a/planners/easynav_simple_planner/CHANGELOG.rst +++ b/planners/easynav_simple_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package easynav_simple_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.0 (2026-07-26) +------------------ * Sync with Rolling branch * Navstate key among filters are always map, not an arbitrary key * Navstate key among filters are always map, not an arbitrary key diff --git a/planners/easynav_simple_planner/package.xml b/planners/easynav_simple_planner/package.xml index fc12820b..5be10b4c 100644 --- a/planners/easynav_simple_planner/package.xml +++ b/planners/easynav_simple_planner/package.xml @@ -2,7 +2,7 @@ easynav_simple_planner - 0.3.1 + 0.4.0 Easy Navigation: Simple planner package. Francisco Martín Rico Apache-2.0 From 29005e57ab41c467708484fed86c06707d140c00 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 31 Jul 2026 14:22:42 +0200 Subject: [PATCH 223/230] Some methods were backported to Humble Signed-off-by: Juan S. Cely G. --- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 4 ++-- localizers/easynav_fusion_localizer/COLCON_IGNORE | 0 .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 4 ++-- .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 4 ++-- .../easynav_bonxai_maps_manager/BonxaiMapsManager.cpp | 8 ++++---- .../CostmapMapsManager.cpp | 8 ++++---- .../tests/costmap_mapsmanager_tests.cpp | 2 +- .../easynav_navmap_maps_manager/NavMapMapsManager.cpp | 10 +++++----- .../OctomapMapsManager.cpp | 8 ++++---- .../easynav_routes_maps_manager/RoutesMapsManager.cpp | 4 ++-- .../filters/RoutesCostmapFilter.cpp | 2 +- .../easynav_simple_maps_manager/SimpleMapsManager.cpp | 8 ++++---- .../src/easynav_costmap_planner/CostmapPlanner.cpp | 2 +- .../src/easynav_navmap_planner/AStarPlanner.cpp | 2 +- .../src/easynav_simple_planner/SimplePlanner.cpp | 2 +- 15 files changed, 34 insertions(+), 34 deletions(-) create mode 100644 localizers/easynav_fusion_localizer/COLCON_IGNORE 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 576c9b74..68b61870 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -272,9 +272,9 @@ AMCLLocalizer::on_initialize() } particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", 10); estimate_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + node->get_node_base_interface()->get_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)); diff --git a/localizers/easynav_fusion_localizer/COLCON_IGNORE b/localizers/easynav_fusion_localizer/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b 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 ae6c00fd..33aa8d87 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -476,9 +476,9 @@ void AMCLLocalizer::on_initialize() } particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", 10); estimate_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/pose", 10); init_pose_sub_ = get_node()->create_subscription( "initialpose", 10, 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 452e06e8..892318ff 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -268,9 +268,9 @@ AMCLLocalizer::on_initialize() std::bind(&AMCLLocalizer::odom_callback, this, _1), options); particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", 10); estimate_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/pose", 10); init_pose_sub_ = get_node()->create_subscription( "initialpose", 10, 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 189c5452..095a4892 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 @@ -66,7 +66,7 @@ BonxaiMapsManager::on_initialize() node->get_parameter(plugin_name + ".resolution", resolution_); bonxai_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); map_path_ = "/tmp/bonxai_map.pcd"; @@ -123,7 +123,7 @@ BonxaiMapsManager::on_initialize() } incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { update_from_pc2(*msg); @@ -131,7 +131,7 @@ BonxaiMapsManager::on_initialize() }); incoming_occ_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_occ_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { @@ -140,7 +140,7 @@ BonxaiMapsManager::on_initialize() }); savemap_srv_ = node->create_service( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/savemap", [this]( const std::shared_ptr request, std::shared_ptr response) 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 1708b7c3..d237ca38 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 @@ -99,11 +99,11 @@ CostmapMapsManager::on_initialize() } base_occ_pub_ = node->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); dynamic_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/dynamic_map", 100); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -129,7 +129,7 @@ CostmapMapsManager::on_initialize() } incoming_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_map", rclcpp::QoS(1).transient_local().reliable(), [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { base_grid_msg_ = *msg; @@ -143,7 +143,7 @@ CostmapMapsManager::on_initialize() }); savemap_srv_ = node->create_service( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", + node->get_name() + std::string("/") + plugin_name + "/savemap", [&]( const std::shared_ptr request, std::shared_ptr response) 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 d962bfd4..f317b3f2 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 @@ -167,7 +167,7 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); - const std::string topic = node->get_fully_qualified_name() + std::string("/test/incoming_map"); + const std::string topic = node->get_name() + std::string("/test/incoming_map"); auto pub = node->create_publisher( topic, rclcpp::QoS(1).transient_local().reliable()); pub->on_activate(); 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 f06cbe7a..7e4fdf23 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 @@ -110,11 +110,11 @@ NavMapMapsManager::on_initialize() } navmap_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); layer_updates_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map_updates", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map_updates", rclcpp::QoS(100)); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -162,7 +162,7 @@ NavMapMapsManager::on_initialize() } incoming_occ_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_occ_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { @@ -176,7 +176,7 @@ NavMapMapsManager::on_initialize() }); incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { @@ -191,7 +191,7 @@ NavMapMapsManager::on_initialize() savemap_srv_ = node->create_service( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/savemap", [this]( const std::shared_ptr request, std::shared_ptr response) 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 8af3a981..58526f33 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 @@ -108,7 +108,7 @@ OctomapMapsManager::on_initialize() // } octomap_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); // if (!package_name.empty() && !occmap_path_file.empty()) { @@ -153,7 +153,7 @@ OctomapMapsManager::on_initialize() // } // incoming_occ_map_sub_ = node->create_subscription( -// node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_occ_map", +// node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_occ_map", // rclcpp::QoS(1).transient_local().reliable(), // [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { // @@ -169,7 +169,7 @@ OctomapMapsManager::on_initialize() const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { @@ -233,7 +233,7 @@ OctomapMapsManager::on_initialize() savemap_srv_ = node->create_service( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/savemap", [this]( const std::shared_ptr request, std::shared_ptr response) 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 f01d753c..edb79b33 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 @@ -94,14 +94,14 @@ void RoutesMapsManager::on_initialize() } routes_pub_ = node->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes", + node->get_name() + std::string("/") + plugin_name + "/routes", rclcpp::QoS(10).transient_local().reliable()); imarker_server_ = std::make_shared( plugin_name + std::string("_imarkers"), node, false); save_routes_srv_ = node->create_service( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/save_routes", + node->get_name() + std::string("/") + plugin_name + "/save_routes", [this](const std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr response) { try { 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 dde4ae1e..c6ba97e4 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 @@ -61,7 +61,7 @@ RoutesCostmapFilter::initialize( (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", + node->get_name() + std::string("/") + plugin_name + "/routes_map", rclcpp::QoS(1).reliable()); } 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 41a35408..e5e455c6 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 @@ -74,16 +74,16 @@ SimpleMapsManager::on_initialize() } static_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); dynamic_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + node->get_node_base_interface()->get_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_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map", + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_map", rclcpp::QoS(1).transient_local().reliable(), [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { static_map_.from_occupancy_grid(*msg); @@ -97,7 +97,7 @@ SimpleMapsManager::on_initialize() }); savemap_srv_ = node->create_service( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", + node->get_name() + std::string("/") + plugin_name + "/savemap", [&]( const std::shared_ptr request, std::shared_ptr response) 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 702f5be8..45a63c76 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -135,7 +135,7 @@ void CostmapPlanner::on_initialize() node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); path_pub_ = node->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); + node->get_name() + std::string("/") + plugin_name + "/path", 10); } void CostmapPlanner::update(NavState & nav_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 3f65c574..30742d99 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -71,7 +71,7 @@ void AStarPlanner::on_initialize() node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); path_pub_ = node->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); + node->get_name() + std::string("/") + plugin_name + "/path", 10); } void AStarPlanner::update(NavState & nav_state) 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 aa8f707d..c18c8e70 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -95,7 +95,7 @@ SimplePlanner::on_initialize() node->get_parameter(plugin_name + ".clearance_distance", clearance_distance_); path_pub_ = get_node()->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); + node->get_name() + std::string("/") + plugin_name + "/path", 10); } void From e57f13d520f0b05cda93a8d236558dc266d9c3af Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 31 Jul 2026 20:33:43 +0200 Subject: [PATCH 224/230] Uncrustify passed Signed-off-by: Juan S. Cely G. --- .../src/easynav_costmap_common/costmap_2d.cpp | 5 +- .../tests/costmap_2d_tests.cpp | 3 +- .../tests/simple_maps_tests.cpp | 3 +- .../easynav_mpc_controller/MPCController.hpp | 4 +- .../easynav_mpc_controller/MPCOptimizer.hpp | 8 +- .../easynav_mpc_controller/MPCController.cpp | 25 ++-- .../easynav_mpc_controller/MPCOptimizer.cpp | 15 +- .../MPPIController.cpp | 8 +- .../easynav_mppi_controller/MPPIOptimizer.cpp | 22 +-- .../dynamic_window_pure_pursuit_functions.hpp | 2 +- .../tests/regulated_pp_controller_tests.cpp | 16 +-- .../SerestController.hpp | 6 +- .../SerestController.cpp | 28 ++-- .../SimpleController.cpp | 24 ++-- .../tests/simple_controller_tests.cpp | 3 +- .../easynav_vff_controller/VffController.cpp | 17 ++- .../AMCLLocalizer.cpp | 25 ++-- .../tests/costmap_localizer_tests.cpp | 3 +- .../FusionLocalizer.cpp | 42 +++--- .../easynav_fusion_localizer/ukf_wrapper.cpp | 134 +++++++++++------- .../tests/fusion_localizer_tests.cpp | 3 +- .../tests/gps_localizer_tests.cpp | 3 +- .../AMCLLocalizer.cpp | 56 +++++--- .../tests/navmap_localizer_tests.cpp | 3 +- .../AMCLLocalizer.cpp | 23 +-- .../tests/simple_localizer_tests.cpp | 3 +- .../include/bonxai/bonxai.hpp | 32 +++-- .../include/bonxai/grid_allocator.hpp | 16 ++- .../include/bonxai/grid_coord.hpp | 24 ++-- .../include/bonxai/mask.hpp | 8 +- .../include/bonxai/probabilistic_map.hpp | 10 +- .../include/bonxai/serialization.hpp | 4 +- .../src/bonxai/probabilistic_map.cpp | 2 +- .../BonxaiMapsManager.cpp | 25 ++-- .../CostmapMapsManager.cpp | 25 ++-- .../filters/InflationFilter.cpp | 3 +- .../tests/costmap_mapsmanager_tests.cpp | 3 +- .../NavMapMapsManager.cpp | 23 +-- .../filters/InflationFilter.cpp | 12 +- .../OctomapMapsManager.cpp | 12 +- .../filters/InflationFilter.cpp | 13 +- .../RoutesMapsManager.cpp | 63 ++++---- .../filters/RoutesCostmapFilter.cpp | 3 +- .../tests/routes_mapsmanager_tests.cpp | 15 +- .../SimpleMapsManager.cpp | 5 +- .../tests/simple_mapsmanager_tests.cpp | 3 +- .../CostmapPlanner.cpp | 24 ++-- .../easynav_simple_planner/SimplePlanner.cpp | 18 ++- 48 files changed, 495 insertions(+), 332 deletions(-) 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 71b19648..1b7a5f14 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 @@ -217,8 +217,9 @@ bool Costmap2D::copyWindow( { const auto source_stamp = source.getLastModifiedStamp(); std::unique_lock lock(*access_); - const int64_t new_stamp_ns = std::max(last_modified_.nanoseconds(), - source_stamp.nanoseconds()) + 1; + const int64_t new_stamp_ns = std::max( + last_modified_.nanoseconds(), + source_stamp.nanoseconds()) + 1; last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } return true; diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index bfef3f94..81983326 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -117,7 +117,8 @@ TEST_F(Costmap2DTest, OccupancyGridConversion) }; for (size_t i = 0; i < grid.data.size(); ++i) { - bool expected = std::find(expected_indices.begin(), + bool expected = std::find( + expected_indices.begin(), expected_indices.end(), i) != expected_indices.end(); EXPECT_EQ(grid.data[i], expected ? 100 : 0); } diff --git a/common/easynav_simple_common/tests/simple_maps_tests.cpp b/common/easynav_simple_common/tests/simple_maps_tests.cpp index 09cd11bb..b51b2d2b 100644 --- a/common/easynav_simple_common/tests/simple_maps_tests.cpp +++ b/common/easynav_simple_common/tests/simple_maps_tests.cpp @@ -121,7 +121,8 @@ TEST_F(SimpleMapTest, OccupancyGridConversion) }; for (std::size_t i = 0; i < grid_msg.data.size(); ++i) { - bool is_occupied = std::find(expected_indices.begin(), + bool is_occupied = std::find( + expected_indices.begin(), expected_indices.end(), i) != expected_indices.end(); EXPECT_EQ(grid_msg.data[i], is_occupied ? 100 : 0); } 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 40e8472d..acef3630 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -66,13 +66,13 @@ class MPCController : public ControllerMethodBase /// \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, + 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); + void collision_checker(void * data, std::vector & u); protected: int horizon_steps_{5}; ///< Prediction horizon for MPC. 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 716225be..40ca6ce6 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -105,7 +105,7 @@ class MPCOptimizer /// \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); + std::vector & grad, void * data); /// \brief Real cost function with static propierties /// \param u std::vector Velocity vector to be optimized @@ -114,15 +114,15 @@ class MPCOptimizer /// \return double cost value used by nlOpt in internal callback static double nlopt_cost_callback( const std::vector & x, - std::vector & grad, void *data); + 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. + 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 ac825d4c..114382e5 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -65,10 +65,10 @@ MPCController::on_initialize() void MPCController::publish_mpc_path( - void *data, const std::vector & best_vel, + void * data, const std::vector & best_vel, nav_msgs::msg::Path path) { - MPCParameters *params = reinterpret_cast(data); + MPCParameters * params = reinterpret_cast(data); nav_msgs::msg::Path mpc_path_; if (best_vel.size() > 0) { @@ -91,30 +91,31 @@ MPCController::publish_mpc_path( } void -MPCController::collision_checker(void *data, std::vector & u) +MPCController::collision_checker(void * data, std::vector & u) { - MPCParameters *params = reinterpret_cast(data); + 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)) { + if (!std::isnan(point.x) || !std::isnan(point.y)) { x_m += (point.x - params->x0[0]); y_m += (point.y - params->x0[1]); real_points++; } } - if(real_points != 0) { + 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_) { + if (dist < safety_radius_) { + if (!collision_state_) { collision_state_ = true; last_v_ = u[0]; last_w_ = u[1]; } - RCLCPP_WARN(get_node()->get_logger(), + 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; @@ -145,7 +146,7 @@ MPCController::update_rt(NavState & nav_state) const auto & perceptions = nav_state.get_no_group(); if (!nav_state.has("path") || !nav_state.has("robot_pose") || perceptions.empty()) { - if(verbose_) { + if (verbose_) { std::cout << "No Path, No Points or No Robot Pose" << std::endl; } return; @@ -255,7 +256,7 @@ MPCController::update_rt(NavState & nav_state) std::vector lb(2 * horizon_steps_); std::vector ub(2 * horizon_steps_); - for (int k = 0; k < horizon_steps_ ; k++) { + 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_; @@ -269,7 +270,7 @@ MPCController::update_rt(NavState & nav_state) try { nlopt::result result = opt.optimize(u, minf); - if(verbose_) { + if (verbose_) { if (result > 0) { std::cerr << "Optimization Successful " << std::endl; std::cout << "Result: " << result << std::endl; 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 c9391bbb..067b1495 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -28,7 +28,7 @@ MPCParameters::MPCParameters( 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; @@ -87,9 +87,9 @@ MPCOptimizer::kinematic_model( double MPCOptimizer::cost_function( const std::vector & u, - [[maybe_unused]] std::vector & grad, void *data) + [[maybe_unused]] std::vector & grad, void * data) { - MPCParameters *params = reinterpret_cast(data); + MPCParameters * params = reinterpret_cast(data); Eigen::Vector3d position = params->x0; Eigen::Vector3d orientation = params->theta0; @@ -107,7 +107,7 @@ MPCOptimizer::cost_function( double v = u[2 * i]; double w = u[2 * i + 1]; double dv, dw; - if(i < (N - 1)) { + if (i < (N - 1)) { dv = u[2 * (i + 1)] - u[2 * i]; dw = u[2 * (i + 1) + 1] - u[2 * i + 1]; } else { @@ -124,7 +124,8 @@ MPCOptimizer::cost_function( Eigen::Vector2d duk(dv, dw); // Tracking cost - cost += Q(0, 0) * error[0] * error[0] + Q(1, + 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; @@ -140,9 +141,9 @@ double MPCOptimizer::nlopt_cost_callback( const std::vector & x, std::vector & grad, - void *data) + void * data) { - auto *cbdata = static_cast(data); + auto * cbdata = static_cast(data); return cbdata->optimizer->cost_function(x, grad, cbdata->params); } 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 f80ca7ff..9947db91 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -59,7 +59,8 @@ MPPIController::on_initialize() node->get_parameter(plugin_name + ".fov", fov_); node->get_parameter(plugin_name + ".safety_radius", safety_radius_); - optimizer_ = std::make_unique(num_samples_, horizon_steps_, dt_, lambda_, + optimizer_ = std::make_unique( + num_samples_, horizon_steps_, dt_, lambda_, max_lin_vel_, max_ang_vel_, fov_, safety_radius_); mppi_candidates_pub_ = @@ -193,8 +194,9 @@ MPPIController::update_rt(NavState & nav_state) .as_points(); if (filtered.empty()) { - RCLCPP_WARN(get_node()->get_logger(), - "No valid points available for MPPI optimization, using the path only."); + RCLCPP_WARN( + get_node()->get_logger(), + "No valid points available for MPPI optimization, using the path only."); } // Compute the control using MPPI with points 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 7e5b9871..2b03cca1 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp @@ -43,8 +43,9 @@ MPPIOptimizer::simulate_trajectory( 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); + 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; @@ -100,12 +101,14 @@ double MPPIOptimizer::compute_cost( 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); + 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 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 @@ -242,8 +245,9 @@ MPPIResult MPPIOptimizer::compute_control( } // 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;}); + 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)]; @@ -266,7 +270,7 @@ MPPIResult MPPIOptimizer::compute_control( denom += sample.cost; } - // Weighted average of velocities + // Weighted average of velocities double vlin = 0.0, vrot = 0.0; for (const auto & sample : samples) { vlin += sample.v * sample.cost / denom; diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp index 3073370c..3579ac33 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp @@ -80,7 +80,7 @@ inline DynamicWindowBounds computeDynamicWindow( // function to compute dynamic window for a single dimension auto compute_window = [&](const double & current_vel, const double & max_vel, - const double & min_vel, const double & max_accel, const double & max_decel) + const double & min_vel, const double & max_accel, const double & max_decel) { double candidate_max_vel = 0.0; double candidate_min_vel = 0.0; diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp index 467b1465..e77fb865 100644 --- a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp @@ -99,10 +99,10 @@ TEST(DynamicWindowPurePursuit, ComputeDynamicWindowClampsToAccelLimits) current_speed.angular.z = 0.0; const auto window = easynav::dynamic_window_pure_pursuit::computeDynamicWindow( - current_speed, /*max_linear_vel=*/1.0, /*min_linear_vel=*/-1.0, - /*max_angular_vel=*/1.0, /*min_angular_vel=*/-1.0, - /*max_linear_accel=*/2.0, /*max_linear_decel=*/2.0, - /*max_angular_accel=*/2.0, /*max_angular_decel=*/2.0, /*dt=*/0.1); + current_speed, /*max_linear_vel=*/ 1.0, /*min_linear_vel=*/ -1.0, + /*max_angular_vel=*/ 1.0, /*min_angular_vel=*/ -1.0, + /*max_linear_accel=*/ 2.0, /*max_linear_decel=*/ 2.0, + /*max_angular_accel=*/ 2.0, /*max_angular_decel=*/ 2.0, /*dt=*/ 0.1); EXPECT_NEAR(window.max_linear_vel, 0.2, 1e-9); EXPECT_NEAR(window.min_linear_vel, -0.2, 1e-9); @@ -118,7 +118,7 @@ TEST(DynamicWindowPurePursuit, ComputeOptimalVelocityZeroCurvatureForward) const auto [lin, ang] = easynav::dynamic_window_pure_pursuit::computeOptimalVelocityWithinDynamicWindow( - window, /*curvature=*/0.0, /*sign=*/1.0); + window, /*curvature=*/ 0.0, /*sign=*/ 1.0); EXPECT_NEAR(lin, 1.0, 1e-9); EXPECT_NEAR(ang, 0.0, 1e-9); @@ -148,7 +148,7 @@ TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointInterpolatesOnSegment) robot.y = 0.0; const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( - path, robot, /*lookahead_dist=*/1.5); + path, robot, /*lookahead_dist=*/ 1.5); EXPECT_NEAR(carrot.x, 1.5, 1e-6); EXPECT_NEAR(carrot.y, 0.0, 1e-6); @@ -172,7 +172,7 @@ TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointIgnoresPathBehindRobot robot.y = 0.0; const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( - path, robot, /*lookahead_dist=*/0.5); + path, robot, /*lookahead_dist=*/ 0.5); // The carrot must be ahead of the robot (close to x=9.5), never back at the path's start (x=0). EXPECT_NEAR(carrot.x, 9.5, 1e-6); @@ -209,7 +209,7 @@ TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointClampsToPathEnd) robot.y = 0.0; const auto carrot = FriendRegulatedPurePursuitController::getLookAheadPoint( - path, robot, /*lookahead_dist=*/5.0); + path, robot, /*lookahead_dist=*/ 5.0); EXPECT_NEAR(carrot.x, 1.0, 1e-6); } 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 5e0d7dee..c854e88c 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -116,7 +116,7 @@ class SerestController : public ControllerMethodBase { size_t seg_idx{0}; ///< Current segment index [i, i+1]. double s_star{0.0}; ///< Arc-length at the closest point. - Vec2 closest{0.0, 0.0}; ///< Closest point coordinates on the path. + Vec2 closest{0.0, 0.0}; ///< Closest point coordinates on the path. double t{0.0}; ///< Segment parameter in [0,1]. }; @@ -259,7 +259,7 @@ class SerestController : public ControllerMethodBase // --- Parameters --- /// @brief Allow reverse motion (if false, forward-only). - bool allow_reverse_{false}; + bool allow_reverse_{false}; /// @brief Minimal forward progress (m/s) when reasonably aligned. double v_progress_min_{0.05}; /// @brief Max fraction of forward progress that -k_s * e_y can cancel. @@ -321,7 +321,7 @@ class SerestController : public ControllerMethodBase double final_align_wmax_{0.6}; /// @brief Enable corner-guard behavior in tight turns. - bool corner_guard_enable_{true}; + bool corner_guard_enable_{true}; /// @brief Corner-guard weight for outside lateral error (e_y_out) in speed reduction. double corner_gain_ey_{1.5}; /// @brief Corner-guard weight for |e_theta| in speed reduction. 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 9e894fd1..30ea299b 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -211,7 +211,7 @@ SerestController::ref_heading_and_curvature( if (i + 1 >= pd.pts.size()) {return v2(1, 0);} return normalize(pd.pts[i + 1] - pd.pts[i]); }; - auto atan2dir = [](const Vec2 & t){return std::atan2(t.y, t.x);}; + auto atan2dir = [](const Vec2 & t) {return std::atan2(t.y, t.x);}; // Relevant segment indices: i-1, i, i + 1 const size_t i = prj.seg_idx; @@ -303,7 +303,8 @@ SerestController::closest_obstacle_distance( auto view = PointPerceptionsOpsView(perceptions); view.downsample(0.3) .fuse(tf_info.robot_frame) - .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); @@ -443,7 +444,7 @@ SerestController::safety_limits( 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); + v_safe = v_safe_from_distance(d_closest, /*slope_sin=*/ 0.0); // Curvature-based limit ("soft" version of the original file) const double ak = std::fabs(rk.kappa_hat); @@ -659,8 +660,9 @@ SerestController::update_rt(NavState & nav_state) // 5) Goal-related terms and slow/stop zone shaping double dist_xy_goal = 0.0, e_theta_goal = 0.0, stop_r = 0.0, slow_r = 0.0, gamma_slow = 1.0; Vec2 goal_xy; double yaw_goal = 0.0; - compute_goal_zone(path, robot_xy, yaw, dist_xy_goal, e_theta_goal, - stop_r, slow_r, gamma_slow, goal_xy, yaw_goal); + compute_goal_zone( + path, robot_xy, yaw, dist_xy_goal, e_theta_goal, + stop_r, slow_r, gamma_slow, goal_xy, yaw_goal); // 6) Global safety limits derived from sensors and curvature double d_closest = 0.0, v_safe = 0.0, v_curv = 0.0; @@ -677,7 +679,8 @@ SerestController::update_rt(NavState & nav_state) 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) - bool tip_request = should_turn_in_place(allow_reverse_, e_theta, e_theta_goal, dist_to_end, + bool tip_request = should_turn_in_place( + allow_reverse_, e_theta, e_theta_goal, dist_to_end, thr_enter); // Additional start-of-path gate: enforce TiP if still near s*=0 and yaw misalignment is large @@ -718,10 +721,10 @@ SerestController::update_rt(NavState & nav_state) publish_cmd_and_debug( nav_state, path, vlin, vrot, e_y, e_theta, rk.kappa_hat, - d_closest, v_safe, v_curv, /*alpha*/1.0, + d_closest, v_safe, v_curv, /*alpha*/ 1.0, allow_reverse_, dist_to_end, dist_xy_goal, gamma_slow, - /*in_final_align*/0, /*arrived*/0); + /*in_final_align*/ 0, /*arrived*/ 0); return; } } @@ -763,7 +766,7 @@ SerestController::update_rt(NavState & nav_state) // 8) Final alignment inside stop zone (publishes and returns if active) if (maybe_final_align_and_publish( - nav_state, path, dist_xy_goal, stop_r, e_theta_goal, gamma_slow, dt)) + nav_state, path, dist_xy_goal, stop_r, e_theta_goal, gamma_slow, dt)) { return; } @@ -791,8 +794,9 @@ SerestController::update_rt(NavState & nav_state) const double s_total = pd.s_acc.back(); const double dist_to_end = s_total - prj.s_star; - if (should_turn_in_place(allow_reverse_, e_theta, e_theta_goal, dist_to_end, - turn_in_place_thr)) + if (should_turn_in_place( + allow_reverse_, e_theta, e_theta_goal, dist_to_end, + turn_in_place_thr)) { v_cmd_raw = 0.0; omega_nom = -k_theta_ * e_theta - k_y_ * std::atan(e_y / std::max(ell_, 1e-3)); @@ -830,7 +834,7 @@ SerestController::update_rt(NavState & nav_state) d_closest, v_safe, v_curv, alpha, allow_reverse_, dist_to_end, dist_xy_goal, gamma_slow, - /*in_final_align=*/0, /*arrived=*/0); + /*in_final_align=*/ 0, /*arrived=*/ 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 6e80af64..d4cd0320 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -46,8 +46,9 @@ 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 + ".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_); @@ -62,8 +63,9 @@ 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 + ".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_); @@ -115,7 +117,8 @@ SimpleController::update_rt(NavState & nav_state) rclcpp::Time latest_stamp( nav_state.get("robot_pose").header.stamp, clock_type); - if (rclcpp::Time(path.poses.back().header.stamp, + if (rclcpp::Time( + path.poses.back().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { latest_stamp = rclcpp::Time(path.poses.back().header.stamp, latest_stamp.get_clock_type()); @@ -136,9 +139,10 @@ SimpleController::update_rt(NavState & nav_state) 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); + 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; } @@ -147,8 +151,8 @@ SimpleController::update_rt(NavState & nav_state) double angle = get_angle(pose.position, ref_pose.position) - tf2::getYaw(pose.orientation); - while(angle > M_PI) {angle -= 2.0 * M_PI;} - while(angle < -M_PI) {angle += 2.0 * M_PI;} + while (angle > M_PI) {angle -= 2.0 * M_PI;} + while (angle < -M_PI) {angle += 2.0 * M_PI;} double vlin = 0.0; double vrot = 0.0; diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index 1b203f2c..c7cade9a 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -131,7 +131,8 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) EXPECT_EQ(static_map->at(1, 1), 0); } -class FriendAMCLLocalizer : public easynav::SimpleController { +class FriendAMCLLocalizer : public easynav::SimpleController +{ public: void force_path(const std::string & path) {map_path_ = path;} }; 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 fc39d89d..2ce95429 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -45,8 +45,9 @@ void VffController::on_initialize() node->declare_parameter(plugin_name + ".max_speed", 0.8); node->declare_parameter(plugin_name + ".max_angular_speed", 1.5); - node->get_parameter(plugin_name + ".distance_obstacle_detection", - distance_obstacle_detection_); + node->get_parameter( + plugin_name + ".distance_obstacle_detection", + distance_obstacle_detection_); node->get_parameter(plugin_name + ".obstacle_detection_x_min", obstacle_detection_x_min_); node->get_parameter(plugin_name + ".obstacle_detection_x_max", obstacle_detection_x_max_); node->get_parameter(plugin_name + ".obstacle_detection_y_min", obstacle_detection_y_min_); @@ -259,9 +260,10 @@ void VffController::update_rt(NavState & nav_state) PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, -10.0}, {10.0, 10.0, 10.0}) .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_}) + .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 @@ -276,8 +278,9 @@ void VffController::update_rt(NavState & nav_state) cmd_vel_.header.stamp = get_node()->now(); cmd_vel_.twist.linear.x = std::clamp(module, 0.0, max_speed_); cmd_vel_.twist.angular.z = std::clamp(angle, -max_angular_speed_, max_angular_speed_); - RCLCPP_INFO(get_node()->get_logger(), "[distance: %.2f, yaw_error: %.2f]", distance, - angle_error); + RCLCPP_INFO( + get_node()->get_logger(), "[distance: %.2f, yaw_error: %.2f]", distance, + angle_error); nav_state.set("cmd_vel", cmd_vel_); } 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 68b61870..65a353b8 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -214,7 +214,8 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); node->get_parameter(plugin_name + ".noise_translation", noise_translation_); node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", + node->get_parameter( + plugin_name + ".noise_translation_to_rotation", noise_translation_to_rotation_); node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); @@ -239,7 +240,8 @@ AMCLLocalizer::on_initialize() } RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", + RCLCPP_INFO( + node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); @@ -272,7 +274,8 @@ AMCLLocalizer::on_initialize() } particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", + 10); estimate_pub_ = get_node()->create_publisher( node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/pose", 10); @@ -501,7 +504,7 @@ AMCLLocalizer::update_odom_from_tf() try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, - tf2::durationFromSec(0.0)); + tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "AMCLLocalizer::update: TF failed: %s", ex.what()); return; @@ -663,7 +666,8 @@ AMCLLocalizer::reseed() const std::size_t N = particles_.size(); const std::size_t N_top = N / 2; - std::sort(particles_.begin(), particles_.end(), + std::sort( + particles_.begin(), particles_.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); @@ -700,8 +704,9 @@ AMCLLocalizer::reseed() double dx = l00 * z0; double dy = l10 * z0 + l11 * z1; - std::normal_distribution xy_noise(0.0, std::max(sqrt(dx * dx + dy * dy), - min_noise_xy_)); + std::normal_distribution xy_noise(0.0, std::max( + sqrt(dx * dx + dy * dy), + min_noise_xy_)); tf2::Vector3 new_origin(origin.x() + xy_noise(rng_), origin.y() + xy_noise(rng_), 0.0); @@ -780,7 +785,8 @@ AMCLLocalizer::getEstimatedPose() const const std::size_t N_top = N / 2; std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), + std::sort( + sorted_particles.begin(), sorted_particles.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); @@ -852,7 +858,8 @@ AMCLLocalizer::get_pose() const std::size_t N_top = N / 2; std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), + std::sort( + sorted_particles.begin(), sorted_particles.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index 0ce41b2e..266b62a6 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -74,7 +74,8 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG const double yaw1 = -1.2; rclcpp::NodeOptions options; - options.parameter_overrides({ + options.parameter_overrides( + { rclcpp::Parameter("test.num_particles", 100), rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), 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 8488f319..b10be598 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -36,8 +36,9 @@ void FusionLocalizer::on_initialize() 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()); + RCLCPP_INFO( + localizer_node->get_logger(), "Using parameter namespace: '%s'", + plugin_name.c_str()); // Detect which filters have parameters configured by checking // parameter overrides (loaded from YAML before declaration) @@ -66,8 +67,8 @@ void FusionLocalizer::on_initialize() "At least one filter must be configured.", global_prefix.c_str(), local_prefix.c_str()); throw std::runtime_error( - "FusionLocalizer: no global_filter or local_filter parameters detected. " - "At least one filter must be configured."); + "FusionLocalizer: no global_filter or local_filter parameters detected. " + "At least one filter must be configured."); } if (has_global_filter_) { @@ -76,8 +77,9 @@ void FusionLocalizer::on_initialize() ); ukf_global_->initialize(); } else { - RCLCPP_WARN(localizer_node->get_logger(), - "No global_filter parameters found. Global filter will NOT be created."); + RCLCPP_WARN( + localizer_node->get_logger(), + "No global_filter parameters found. Global filter will NOT be created."); } if (has_local_filter_) { @@ -86,8 +88,9 @@ void FusionLocalizer::on_initialize() ); ukf_local_->initialize(); } else { - RCLCPP_WARN(localizer_node->get_logger(), - "No local_filter parameters found. Local filter will NOT be created."); + RCLCPP_WARN( + localizer_node->get_logger(), + "No local_filter parameters found. Local filter will NOT be created."); } // GPS-related setup only needed when global filter is active @@ -116,8 +119,9 @@ void FusionLocalizer::on_initialize() } if (has_global_filter_) { - GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_, - UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); + GeographicLib::UTMUPS::Forward( + latitude_origin_, longitude_origin_, UTM_zone_number_, + UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S"); UTM_origin_z_ = altitude_origin_; @@ -142,7 +146,8 @@ void FusionLocalizer::init_pose_callback( UTM_origin_x_ += (current_x - msg->pose.pose.position.x); UTM_origin_y_ += (current_y - msg->pose.pose.position.y); - RCLCPP_INFO(get_node()->get_logger(), + RCLCPP_INFO( + get_node()->get_logger(), "Initial pose reset UTM origin. Shifted X by %.2f, Y by %.2f to match pose (%.2f, %.2f)", (current_x - msg->pose.pose.position.x), (current_y - msg->pose.pose.position.y), msg->pose.pose.position.x, msg->pose.pose.position.y); @@ -175,11 +180,13 @@ void FusionLocalizer::update_rt(NavState & nav_state) EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); last_gps_stamp_[i] = gps_time; auto pose = - std::make_shared(navsatfix_to_pose( + std::make_shared( + navsatfix_to_pose( gps_data[i]->data)); if (!first_pose_received_) { - RCLCPP_INFO(get_node()->get_logger(), - "First valid GPS fix received. Initializing filter state."); + RCLCPP_INFO( + get_node()->get_logger(), + "First valid GPS fix received. Initializing filter state."); if (nav_state.has("imu")) { auto imu_data = nav_state.get(std::string("imu")); pose->pose.pose.orientation = imu_data.data.orientation; @@ -270,7 +277,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.pose.covariance[28] = default_var; pose_msg.pose.covariance[35] = default_var; } else { - // Fallback variances if GPS doesn't provide them + // Fallback variances if GPS doesn't provide them pose_msg.pose.covariance[0] = default_var; pose_msg.pose.covariance[7] = default_var; pose_msg.pose.covariance[14] = default_var; @@ -279,8 +286,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.pose.covariance[28] = default_var; pose_msg.pose.covariance[35] = default_var; - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, - "NavSatFix covariance type unknown or invalid. Using default covariance."); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 5000, + "NavSatFix covariance type unknown or invalid. Using default covariance."); } return pose_msg; 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 48e2ce20..e4d52a0c 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 @@ -865,8 +865,9 @@ 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 @@ -893,7 +894,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 - if(local_filter_) { + if (local_filter_) { world_frame_id_ = tf_info.odom_frame; } else { world_frame_id_ = tf_info.map_frame; @@ -954,12 +955,14 @@ 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); @@ -972,11 +975,14 @@ 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", + rclcpp::Duration::from_seconds( + parent_node_->declare_parameter( + param_prefix + "sensor_timeout", 1.0 / frequency_)); filter_.setSensorTimeout(sensor_timeout_); @@ -985,8 +991,9 @@ 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( @@ -1038,8 +1045,9 @@ 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( @@ -1057,8 +1065,9 @@ 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) { @@ -1073,8 +1082,9 @@ 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( @@ -1091,8 +1101,9 @@ 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) { @@ -1120,7 +1131,8 @@ 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); @@ -1141,8 +1153,9 @@ 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 @@ -1246,13 +1259,15 @@ 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( @@ -1264,23 +1279,27 @@ 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); @@ -1401,12 +1420,14 @@ 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); @@ -1420,13 +1441,15 @@ 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); @@ -1519,12 +1542,14 @@ 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); @@ -1538,7 +1563,8 @@ 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()); @@ -1642,13 +1668,15 @@ 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); @@ -1721,13 +1749,15 @@ 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( @@ -1739,7 +1769,8 @@ 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()); @@ -1747,17 +1778,20 @@ 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); @@ -1765,7 +1799,8 @@ 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); @@ -1965,7 +2000,7 @@ void UkfWrapper::loadParams() deceleration_gains); // Select between TwistStamped or Twist control input - if(stamped_control_) { + if (stamped_control_) { stamped_control_sub_ = parent_node_->create_subscription( "cmd_vel", rclcpp::QoS(1), std::bind(&UkfWrapper::controlStampedCallback, this, std::placeholders::_1), options); @@ -2056,7 +2091,8 @@ void UkfWrapper::loadParams() filter_.setProcessNoiseCovariance(process_noise_covariance_); } - if (load_covariance(param_prefix + "initial_estimate_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"); diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp index d010b352..d43ae9a9 100644 --- a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp @@ -71,7 +71,8 @@ TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbac // Provide at least one parameter override so FusionLocalizer considers a filter configured. rclcpp::NodeOptions options; - options.parameter_overrides({ + options.parameter_overrides( + { rclcpp::Parameter("test.global_filter.frequency", 30.0), }); diff --git a/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp index b594eaee..109266e2 100644 --- a/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp +++ b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp @@ -59,7 +59,8 @@ TEST_F(GpsLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGr const double y1 = 0.4; rclcpp::NodeOptions options; - options.parameter_overrides({ + options.parameter_overrides( + { rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), rclcpp::Parameter("test.initial_pose.yaw", 0.0), 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 33aa8d87..7f99c641 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -165,13 +165,13 @@ static inline tf2::Vector3 to_tf(const Eigen::Vector3f & v) } // ===================== probabilistic inflate (brushfire) ===================== -using ProgressCallback = std::function; +using ProgressCallback = std::function; /// Expect the fixed comma value returned by logods() [[nodiscard]] static constexpr float prob(int32_t logods_fixed) { float logods = float(logods_fixed) * 1e-6; - return 1.0 - 1.0 / (1.0 + std::exp(logods)); + return 1.0 - 1.0 / (1.0 + std::exp(logods)); } std::shared_ptr @@ -234,7 +234,7 @@ inflate_map( const auto clamp_min = dst->options().clamp_min_log; const auto clamp_max = dst->options().clamp_max_log; - auto write_max = [&](const CoordT & c, int32_t v){ + auto write_max = [&](const CoordT & c, int32_t v) { if (v == std::numeric_limits::min()) {return;} auto * cell = acc.value(c, true); int32_t prop = std::max(cell->probability_log, v); @@ -350,8 +350,9 @@ AMCLLocalizer::AMCLLocalizer() NavState::register_printer( [](const nav_msgs::msg::Odometry & odom) { std::ostringstream ret; - tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); + 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 << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " << odom.pose.pose.position.x @@ -399,8 +400,9 @@ void AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); node->get_parameter(plugin_name + ".noise_translation", noise_translation_); node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", - noise_translation_to_rotation_); + node->get_parameter( + plugin_name + ".noise_translation_to_rotation", + noise_translation_to_rotation_); node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); node->get_parameter(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); @@ -444,10 +446,12 @@ void AMCLLocalizer::on_initialize() reseed_time_ = 1.0 / reseed_freq; RCLCPP_INFO(node->get_logger(), "Initialized AMCL with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "init pose (%.3f, %.3f, %.3f) std_dev [%.3f, %.3f]", + RCLCPP_INFO( + node->get_logger(), "init pose (%.3f, %.3f, %.3f) std_dev [%.3f, %.3f]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - RCLCPP_INFO(node->get_logger(), "inflation_stddev=%.3f, inflation_prob_min=%.3f, " - "correct_max_points=%zu, weights_tau=%.3f, top_keep_fraction=%.3f", + RCLCPP_INFO( + node->get_logger(), "inflation_stddev=%.3f, inflation_prob_min=%.3f, " + "correct_max_points=%zu, weights_tau=%.3f, top_keep_fraction=%.3f", inflation_stddev_, inflation_prob_min_, correct_max_points_, weights_tau_, top_keep_fraction_); std::normal_distribution noise_x(x_init, std_dev_xy); @@ -476,7 +480,8 @@ void AMCLLocalizer::on_initialize() } particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", + 10); estimate_pub_ = get_node()->create_publisher( node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/pose", 10); @@ -650,8 +655,8 @@ 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, - tf2::durationFromSec(0.0)); + tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "TF failed: %s", ex.what()); return; @@ -734,8 +739,9 @@ void AMCLLocalizer::predict(NavState & nav_state) Eigen::Vector3f bary, hit_eig; const bool ok = navmap.locate_navcel( - Eigen::Vector3f(static_cast(Pw.x()), static_cast(Pw.y()), - static_cast(Pw.z())), + Eigen::Vector3f( + static_cast(Pw.x()), static_cast(Pw.y()), + static_cast(Pw.z())), sidx, cid, bary, &hit_eig, opts); if (ok) { @@ -792,7 +798,7 @@ static inline tf2::Transform lookup_bf_to_sensor( if (sensor_frame.empty()) {return tf2::Transform::getIdentity();} geometry_msgs::msg::TransformStamped tf_msg = RTTFBuffer::getInstance()->lookupTransform( - robot_frame, 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; } @@ -873,8 +879,9 @@ void AMCLLocalizer::correct(NavState & nav_state) auto logger = get_node()->get_logger(); auto clock = get_node()->get_clock(); auto progress_cb = [logger, clock](float f) { - RCLCPP_INFO_THROTTLE(logger, *clock, 2000, "Inflating... %.1f%%", - static_cast(f) * 100.0); + RCLCPP_INFO_THROTTLE( + logger, *clock, 2000, "Inflating... %.1f%%", + static_cast(f) * 100.0); }; std::shared_ptr inflated_map = inflate_map(original_map, inflation_stddev_, inflation_prob_min_, progress_cb); @@ -911,14 +918,16 @@ 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_frame, + b.frame_id); // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_node()->get_logger(), "TF bf->%s failed: %s", b.frame_id.c_str(), - ex.what()); + RCLCPP_WARN( + get_node()->get_logger(), "TF bf->%s failed: %s", b.frame_id.c_str(), + ex.what()); T_bf_sensor_cache[b.frame_id] = tf2::Transform::getIdentity(); } } @@ -979,7 +988,8 @@ AMCLLocalizer::reseed() }; const std::size_t N_top = top_count(N); - std::sort(particles_.begin(), particles_.end(), + std::sort( + particles_.begin(), particles_.end(), [](const Particle & a, const Particle & b) {return a.weight > b.weight;}); tf2::Vector3 mean = computeMean(particles_, 0, N_top); diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp index 720c89f7..7b454a4e 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp @@ -72,7 +72,8 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG const double yaw1 = 2.0; rclcpp::NodeOptions options; - options.parameter_overrides({ + options.parameter_overrides( + { rclcpp::Parameter("test.num_particles", 100), rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), 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 892318ff..3f9674e2 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -211,7 +211,8 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); node->get_parameter(plugin_name + ".noise_translation", noise_translation_); node->get_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->get_parameter(plugin_name + ".noise_translation_to_rotation", + node->get_parameter( + plugin_name + ".noise_translation_to_rotation", noise_translation_to_rotation_); node->get_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); @@ -236,7 +237,8 @@ AMCLLocalizer::on_initialize() reseed_time_ = 1.0 / reseed_freq; RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles); - RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", + RCLCPP_INFO( + node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); @@ -268,7 +270,8 @@ AMCLLocalizer::on_initialize() std::bind(&AMCLLocalizer::odom_callback, this, _1), options); particles_pub_ = get_node()->create_publisher( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", 10); + node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/particles", + 10); estimate_pub_ = get_node()->create_publisher( node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/pose", 10); @@ -619,7 +622,8 @@ AMCLLocalizer::reseed() const std::size_t N = particles_.size(); const std::size_t N_top = N / 2; - std::sort(particles_.begin(), particles_.end(), + std::sort( + particles_.begin(), particles_.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); @@ -656,8 +660,9 @@ AMCLLocalizer::reseed() double dx = l00 * z0; double dy = l10 * z0 + l11 * z1; - std::normal_distribution xy_noise(0.0, std::max(sqrt(dx * dx + dy * dy), - min_noise_xy_)); + std::normal_distribution xy_noise(0.0, std::max( + sqrt(dx * dx + dy * dy), + min_noise_xy_)); tf2::Vector3 new_origin(origin.x() + xy_noise(rng_), origin.y() + xy_noise(rng_), 0.0); @@ -737,7 +742,8 @@ AMCLLocalizer::getEstimatedPose() const const std::size_t N_top = N / 2; std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), + std::sort( + sorted_particles.begin(), sorted_particles.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); @@ -810,7 +816,8 @@ AMCLLocalizer::get_pose() const std::size_t N_top = N / 2; std::vector sorted_particles = particles_; - std::sort(sorted_particles.begin(), sorted_particles.end(), + std::sort( + sorted_particles.begin(), sorted_particles.end(), [](const Particle & a, const Particle & b) { return a.weight > b.weight; }); diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index 12dd1636..d122a9ec 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -72,7 +72,8 @@ TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackG const double yaw1 = 1.1; rclcpp::NodeOptions options; - options.parameter_overrides({ + options.parameter_overrides( + { rclcpp::Parameter("test.num_particles", 100), rclcpp::Parameter("test.initial_pose.x", x0), rclcpp::Parameter("test.initial_pose.y", y0), diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/bonxai.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/bonxai.hpp index cf9fee79..82fd94fb 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/bonxai.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/bonxai.hpp @@ -38,7 +38,8 @@ struct EmptyVoxel {}; * DIM will be 8 and SIZE 512 (8³) */ template -class Grid { +class Grid +{ private: uint8_t dim_ = 0; // total number of elements in the cube @@ -98,14 +99,14 @@ class Grid { [[nodiscard]] DataT & cell(size_t index) { static_assert( - !std::is_same_v, "Cells have no value, when DataT == EmptyVoxel"); + !std::is_same_v, "Cells have no value, when DataT == EmptyVoxel"); return data_[index]; } [[nodiscard]] const DataT & cell(size_t index) const { static_assert( - !std::is_same_v, "Cells have no value, when DataT == EmptyVoxel"); + !std::is_same_v, "Cells have no value, when DataT == EmptyVoxel"); return data_[index]; } }; @@ -121,7 +122,8 @@ enum ClearOption }; template -class VoxelGrid { +class VoxelGrid +{ private: uint32_t INNER_BITS; uint32_t LEAF_BITS; @@ -228,7 +230,8 @@ class VoxelGrid { // for special cases, such as serialization or testing std::shared_ptr allocateLeafGrid(); - class ConstAccessor { + class ConstAccessor + { public: ConstAccessor(const VoxelGrid & grid) : grid_(grid) {} @@ -273,7 +276,8 @@ class VoxelGrid { * * Create an instance of this object with the method VoxelGrid::createAccessor() */ - class Accessor : public ConstAccessor { + class Accessor : public ConstAccessor + { public: Accessor(VoxelGrid & grid) : ConstAccessor(grid), @@ -481,9 +485,9 @@ template inline bool VoxelGrid::Accessor::setValue(const CoordT & coord, const DataT & value) { static_assert( - !std::is_same_v, - "You can not access a value when using type EmptyVoxel. Use " - "setCellOn / setCellOff"); + !std::is_same_v, + "You can not access a value when using type EmptyVoxel. Use " + "setCellOn / setCellOff"); const CoordT inner_key = mutable_grid_.getInnerKey(coord); if (inner_key != prev_inner_coord_ || prev_leaf_ptr_ == nullptr) { @@ -502,9 +506,9 @@ template inline DataT * VoxelGrid::Accessor::value(const CoordT & coord, bool create_if_missing) { static_assert( - !std::is_same_v, - "You can not access a value when using type EmptyVoxel. Use " - "isCellOn / setCellOn / setCellOff"); + !std::is_same_v, + "You can not access a value when using type EmptyVoxel. Use " + "isCellOn / setCellOn / setCellOff"); const CoordT inner_key = mutable_grid_.getInnerKey(coord); @@ -530,8 +534,8 @@ template inline const DataT * VoxelGrid::ConstAccessor::value(const CoordT & coord) const { static_assert( - !std::is_same_v, - "You can not access a value when using type EmptyVoxel. Use isCellOn"); + !std::is_same_v, + "You can not access a value when using type EmptyVoxel. Use isCellOn"); const CoordT inner_key = grid_.getInnerKey(coord); diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_allocator.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_allocator.hpp index 4c7ab7e7..5da57f3b 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_allocator.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_allocator.hpp @@ -27,7 +27,8 @@ namespace Bonxai * Each chunk allocates memory for 512 Grids */ template -class GridBlockAllocator { +class GridBlockAllocator +{ public: // use log2dim of the block to be allocated GridBlockAllocator(size_t log2dim); @@ -38,7 +39,7 @@ class GridBlockAllocator { GridBlockAllocator & operator=(const GridBlockAllocator & other) = delete; GridBlockAllocator & operator=(GridBlockAllocator && other) = default; - using Deleter = std::function; + using Deleter = std::function; std::pair allocateBlock(); @@ -133,11 +134,12 @@ inline void GridBlockAllocator::releaseUnusedMemory() { std::unique_lock lock(*mutex_); int to_be_erased_count = 0; - auto remove_if = std::remove_if(chunks_.begin(), chunks_.end(), [&](const auto & chunk) -> bool { - bool notUsed = chunk->mask.isOn(); - to_be_erased_count += (notUsed) ? 1 : 0; - return notUsed; - }); + auto remove_if = std::remove_if( + chunks_.begin(), chunks_.end(), [&](const auto & chunk) -> bool { + bool notUsed = chunk->mask.isOn(); + to_be_erased_count += (notUsed) ? 1 : 0; + return notUsed; + }); chunks_.erase(remove_if, chunks_.end()); capacity_ -= to_be_erased_count * blocks_per_chunk; } diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_coord.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_coord.hpp index ca1f2b28..264d0831 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_coord.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/grid_coord.hpp @@ -143,17 +143,19 @@ template inline PointOut ConvertPoint(const PointIn & v) { // clang-format off - static_assert(std::is_same_v|| - type_has_method_x::value || - type_has_member_x::value || - type_is_vector::value, - "Can't convert from the specified type"); - - static_assert(std::is_same_v|| - type_has_method_x::value || - type_has_member_x::value || - type_is_vector::value, - "Can't convert to the specified type"); + static_assert( + std::is_same_v|| + type_has_method_x::value || + type_has_member_x::value || + type_is_vector::value, + "Can't convert from the specified type"); + + static_assert( + std::is_same_v|| + type_has_method_x::value || + type_has_member_x::value || + type_is_vector::value, + "Can't convert to the specified type"); // clang-format on if constexpr (std::is_same_v) { diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/mask.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/mask.hpp index dac2dc51..09c5957d 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/mask.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/mask.hpp @@ -17,7 +17,8 @@ namespace Bonxai { -class Mask { +class Mask +{ uint64_t * words_ = nullptr; // small object optimization, that will be used when // SIZE <= 512 bits, i.e LOG2DIM <= 3 @@ -61,7 +62,8 @@ class Mask { uint32_t countOn() const; - class Iterator { + class Iterator + { public: Iterator(const Mask * parent) : pos_(parent->SIZE), @@ -268,7 +270,7 @@ inline uint32_t Mask::findNextOn(uint32_t start) const while (!b && ++n < WORD_COUNT) { b = words_[n]; } // find next non-zero word - return !b ? SIZE : (n << 6) + FindLowestOn(b); // catch last word=0 + return !b ? SIZE : (n << 6) + FindLowestOn(b); // catch last word=0 } inline Mask::Mask(size_t log2dim) diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/probabilistic_map.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/probabilistic_map.hpp index 741b54ed..c056f3ca 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/probabilistic_map.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/probabilistic_map.hpp @@ -14,10 +14,11 @@ void RayIterator(const CoordT & key_origin, const CoordT & key_end, const Functo inline void ComputeRay(const CoordT & key_origin, const CoordT & key_end, std::vector & ray) { ray.clear(); - RayIterator(key_origin, key_end, [&ray](const CoordT & coord) { + RayIterator( + key_origin, key_end, [&ray](const CoordT & coord) { ray.push_back(coord); return true; - }); + }); } /** @@ -26,7 +27,8 @@ inline void ComputeRay(const CoordT & key_origin, const CoordT & key_end, std::v * * Insert a point cloud to update the current probability */ -class ProbabilisticMap { +class ProbabilisticMap +{ public: using Vector3D = Eigen::Vector3d; @@ -42,7 +44,7 @@ class ProbabilisticMap { [[nodiscard]] static constexpr float prob(int32_t logods_fixed) { float logods = float(logods_fixed) * 1e-6; - return 1.0 - 1.0 / (1.0 + std::exp(logods)); + return 1.0 - 1.0 / (1.0 + std::exp(logods)); } struct CellT diff --git a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/serialization.hpp b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/serialization.hpp index 0eecce6c..d7440b29 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/bonxai/serialization.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/bonxai/serialization.hpp @@ -89,8 +89,8 @@ inline void Serialize(std::ostream & out, const VoxelGrid & grid) std::string type_name = details::demangle(typeid(DataT).name()); sprintf( - header, "Bonxai::VoxelGrid<%s,%d,%d>(%lf)\n", type_name.c_str(), grid.innetBits(), - grid.leafBits(), grid.voxelSize()); + header, "Bonxai::VoxelGrid<%s,%d,%d>(%lf)\n", type_name.c_str(), grid.innetBits(), + grid.leafBits(), grid.voxelSize()); out.write(header, std::strlen(header)); diff --git a/maps_managers/easynav_bonxai_maps_manager/src/bonxai/probabilistic_map.cpp b/maps_managers/easynav_bonxai_maps_manager/src/bonxai/probabilistic_map.cpp index 036d9a5b..7f168809 100644 --- a/maps_managers/easynav_bonxai_maps_manager/src/bonxai/probabilistic_map.cpp +++ b/maps_managers/easynav_bonxai_maps_manager/src/bonxai/probabilistic_map.cpp @@ -86,7 +86,7 @@ bool ProbabilisticMap::isFree(const CoordT & coord) const float ProbabilisticMap::queryProbability(const Bonxai::CoordT & coord) const { - if (const CellT * cell = _accessor.value(coord, /*create=*/false)) { + if (const CellT * cell = _accessor.value(coord, /*create=*/ false)) { float p = ProbabilisticMap::prob(cell->probability_log); return std::clamp(p, 0.0f, 1.0f); } 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 095a4892..4f32a408 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 @@ -88,11 +88,12 @@ BonxaiMapsManager::on_initialize() if (!std::isfinite(p.x()) || !std::isfinite(p.y()) || !std::isfinite(p.z())) {continue;} bonxai_map_->addHitPoint(p); - pcl_out.push_back({ + pcl_out.push_back( + { static_cast(p.x()), static_cast(p.y()), static_cast(p.z()) - }); + }); } bonxai_msg_.data.clear(); @@ -123,7 +124,8 @@ BonxaiMapsManager::on_initialize() } incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { update_from_pc2(*msg); @@ -131,7 +133,8 @@ BonxaiMapsManager::on_initialize() }); incoming_occ_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_occ_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { @@ -173,8 +176,8 @@ BonxaiMapsManager::update_from_pc2(const sensor_msgs::msg::PointCloud2 & pc2) geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = ::easynav::RTTFBuffer::getInstance()->lookupTransform( - tf_info.map_frame, pc2.header.frame_id, pc2.header.stamp, - rclcpp::Duration::from_seconds(0.05)); + tf_info.map_frame, pc2.header.frame_id, pc2.header.stamp, + rclcpp::Duration::from_seconds(0.05)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "OctomapMapsManager: TF failed: %s", ex.what()); return; @@ -202,11 +205,12 @@ BonxaiMapsManager::update_from_pc2(const sensor_msgs::msg::PointCloud2 & pc2) bonxai_result.clear(); bonxai_map_->getOccupiedVoxels(bonxai_result); for (const auto & voxel : bonxai_result) { - pcl_out.push_back({ + pcl_out.push_back( + { static_cast(voxel.x()), static_cast(voxel.y()), static_cast(voxel.z()) - }); + }); } bonxai_msg_.data.clear(); @@ -264,11 +268,12 @@ BonxaiMapsManager::update_from_occ(const nav_msgs::msg::OccupancyGrid & occ) bonxai_result.clear(); bonxai_map_->getOccupiedVoxels(bonxai_result); for (const auto & voxel : bonxai_result) { - pcl_out.push_back({ + pcl_out.push_back( + { static_cast(voxel.x()), static_cast(voxel.y()), static_cast(voxel.z()) - }); + }); } bonxai_msg_.data.clear(); 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 d237ca38..0d6d77c3 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 @@ -71,7 +71,8 @@ CostmapMapsManager::on_initialize() node->get_parameter(plugin_name + "." + costmap_filter + ".plugin", plugin); try { - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "Loading CostmapFilter %s [%s]", costmap_filter.c_str(), plugin.c_str()); std::shared_ptr instance; @@ -80,21 +81,26 @@ CostmapMapsManager::on_initialize() try { instance->initialize(node, plugin_name + "." + costmap_filter); } catch (std::runtime_error & ex) { - RCLCPP_ERROR(node->get_logger(), + RCLCPP_ERROR( + node->get_logger(), "Unable to initialize [%s]. Error: %s", plugin.c_str(), ex.what()); - throw std::runtime_error("Unable to initialize " + - plugin + " . Error: " + ex.what()); + throw std::runtime_error( + "Unable to initialize " + + plugin + " . Error: " + ex.what()); } costmap_filters_.push_back(instance); - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "Loaded CostmapFilter %s [%s]", costmap_filter.c_str(), plugin.c_str()); } catch (pluginlib::PluginlibException & ex) { - RCLCPP_ERROR(node->get_logger(), + RCLCPP_ERROR( + node->get_logger(), "Unable to load plugin easynav::CostmapFilter. Error: %s", ex.what()); - throw std::runtime_error("Unable to load plugin easynav::CostmapFilter " + - costmap_filter + " . Error: " + ex.what()); + throw std::runtime_error( + "Unable to load plugin easynav::CostmapFilter " + + costmap_filter + " . Error: " + ex.what()); } } @@ -103,7 +109,8 @@ CostmapMapsManager::on_initialize() rclcpp::QoS(1).transient_local().reliable()); dynamic_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/dynamic_map", 100); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 f03c72d1..bde4337a 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 @@ -86,7 +86,8 @@ InflationFilter::on_initialize() 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(), + RCLCPP_INFO( + node->get_logger(), "InflationFilter with inflation_radius = %lf inscribed_radius = %lf cost_scaling_factor = %lf", inflation_radius_, inscribed_radius_, cost_scaling_factor_); 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 f317b3f2..c6e9997e 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 @@ -191,7 +191,8 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getCost(5, 5), easynav::LETHAL_OBSTACLE); - EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), + EXPECT_EQ( + dyn_ptr->getLastModifiedStamp().nanoseconds(), base_after.getLastModifiedStamp().nanoseconds()); } 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 7e4fdf23..2d15c28a 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 @@ -84,7 +84,8 @@ NavMapMapsManager::on_initialize() node->get_parameter(plugin_name + "." + navmap_filter + ".plugin", plugin); try { - RCLCPP_INFO(node->get_logger(), + 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); @@ -92,20 +93,24 @@ NavMapMapsManager::on_initialize() try { instance->initialize(node, plugin_name + "." + navmap_filter); } catch (const std::runtime_error & ex) { - RCLCPP_ERROR(node->get_logger(), + RCLCPP_ERROR( + node->get_logger(), "Unable to initialize [%s]. Error: %s", plugin.c_str(), ex.what()); throw; } navmap_filters_.push_back(instance); - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "Loaded NavMapFilter %s [%s]", navmap_filter.c_str(), plugin.c_str()); } catch (pluginlib::PluginlibException & ex) { - RCLCPP_ERROR(node->get_logger(), + RCLCPP_ERROR( + node->get_logger(), "Unable to load plugin easynav::navmap::NavMapFilter. Error: %s", ex.what()); - throw std::runtime_error("Unable to load plugin easynav::navmap::NavMapFilter " + - navmap_filter + " . Error: " + ex.what()); + throw std::runtime_error( + "Unable to load plugin easynav::navmap::NavMapFilter " + + navmap_filter + " . Error: " + ex.what()); } } @@ -162,7 +167,8 @@ NavMapMapsManager::on_initialize() } incoming_occ_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_occ_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { @@ -176,7 +182,8 @@ NavMapMapsManager::on_initialize() }); incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), [&](sensor_msgs::msg::PointCloud2::UniquePtr 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 33d6c950..9ae969f8 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 @@ -84,16 +84,17 @@ bool InflationFilter::inflate_layer_u8( } // Destination layer (create if missing) - auto dst_view = nm.layers.add_or_get(dst_layer, nm.navcels.size(), - ::navmap::layer_type_tag()); + 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 size_t N = nm.navcels.size(); - const float R = inflation_radius; - 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); // Precompute XY centroids for each NavCel std::vector C(N); @@ -213,7 +214,8 @@ InflationFilter::on_initialize() 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(), + RCLCPP_INFO( + node->get_logger(), "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f", inflation_radius_, cost_scaling_factor_, inscribed_radius_); } 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 58526f33..1379bb7b 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 @@ -169,7 +169,8 @@ OctomapMapsManager::on_initialize() const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); incoming_pc2_map_sub_ = node->create_subscription( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/incoming_pc2_map", + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { @@ -177,7 +178,7 @@ OctomapMapsManager::on_initialize() try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( tf_info.map_frame, msg->header.frame_id, msg->header.stamp, - rclcpp::Duration::from_seconds(0.05)); + rclcpp::Duration::from_seconds(0.05)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "OctomapMapsManager: TF failed: %s", ex.what()); return; @@ -192,9 +193,10 @@ OctomapMapsManager::on_initialize() pcl::fromROSMsg(*msg, *pcl_ds); pcl::VoxelGrid vg; - vg.setLeafSize(static_cast(resolution), - static_cast(resolution), - static_cast(resolution)); + vg.setLeafSize( + static_cast(resolution), + static_cast(resolution), + static_cast(resolution)); vg.filter(*pcl_ds); octomap_ = std::make_shared<::octomap::OcTree>(resolution); 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 09a13363..949c5a83 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 @@ -85,8 +85,9 @@ bool InflationFilter::inflate_layer_u8( 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()); + auto dst_view = nm.layers.add_or_get( + dst_layer, nm.navcels.size(), + 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); @@ -193,7 +194,8 @@ InflationFilter::on_initialize() 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(), + RCLCPP_INFO( + node->get_logger(), "InflationFilter with inflation_radius = %lf cost_scaling_factor = %lf", inflation_radius_, cost_scaling_factor_); } @@ -207,8 +209,9 @@ InflationFilter::update(::easynav::NavState & nav_state) octomap_ = nav_state.get<::octomap::Octomap>("map"); - if (!inflate_layer_u8(octomap_, "obstacles", "inflated_obstacles", - inflation_radius_, cost_scaling_factor_, 0.3)) + if (!inflate_layer_u8( + octomap_, "obstacles", "inflated_obstacles", + inflation_radius_, cost_scaling_factor_, 0.3)) { RCLCPP_ERROR(parent_node_->get_logger(), "Error inflating at ObstacleFilter"); } 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 edb79b33..10eac533 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 @@ -37,12 +37,12 @@ RoutesMapsManager::RoutesMapsManager() 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 << ")"; + out << "\n [" << i << "] from (" << s.start.position.x << ", " + << s.start.position.y << ") to (" << s.end.position.x << ", " + << s.end.position.y << ")"; } return out.str(); - }); + }); routes_filters_loader_ = std::make_unique>( "easynav_routes_maps_manager", "easynav::RoutesFilter"); @@ -89,8 +89,8 @@ void RoutesMapsManager::on_initialize() map_path_ = (pkgpath / map_path_file).string(); } else { throw std::runtime_error( - "Parameters '" + plugin_name + ".package' and '" + plugin_name + - ".map_path_file' are not correctly set"); + "Parameters '" + plugin_name + ".package' and '" + plugin_name + + ".map_path_file' are not correctly set"); } routes_pub_ = node->create_publisher( @@ -196,13 +196,15 @@ void RoutesMapsManager::on_initialize() node->get_parameter(plugin_name + "." + filter_name + ".plugin", plugin); if (plugin.empty()) { - RCLCPP_WARN(node->get_logger(), + RCLCPP_WARN( + node->get_logger(), "RoutesMapsManager: plugin parameter for filter '%s' is empty", filter_name.c_str()); continue; } try { - RCLCPP_INFO(node->get_logger(), + RCLCPP_INFO( + node->get_logger(), "Loading RoutesFilter %s [%s]", filter_name.c_str(), plugin.c_str()); std::shared_ptr instance = @@ -211,22 +213,27 @@ void RoutesMapsManager::on_initialize() try { instance->initialize(node, plugin_name + "." + filter_name); } catch (const std::exception & e) { - RCLCPP_ERROR(node->get_logger(), + RCLCPP_ERROR( + node->get_logger(), "Unable to initialize RoutesFilter %s [%s]. Error: %s", filter_name.c_str(), plugin.c_str(), e.what()); - throw std::runtime_error("Unable to initialize RoutesFilter " + plugin + - " . Error: " + e.what()); + throw std::runtime_error( + "Unable to initialize RoutesFilter " + plugin + + " . Error: " + e.what()); } routes_filters_.push_back(instance); - RCLCPP_INFO(node->get_logger(), + 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(), + RCLCPP_ERROR( + node->get_logger(), "Unable to load plugin easynav::RoutesFilter. Error: %s", ex.what()); - throw std::runtime_error("Unable to load plugin easynav::RoutesFilter " + - filter_name + " . Error: " + ex.what()); + throw std::runtime_error( + "Unable to load plugin easynav::RoutesFilter " + + filter_name + " . Error: " + ex.what()); } } } @@ -392,8 +399,8 @@ 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. + // First, delete all previous markers in our namespaces so that + // removed segments do not leave orphaned markers behind. { visualization_msgs::msg::Marker m; m.header.frame_id = tf_info.map_frame; @@ -562,7 +569,7 @@ void RoutesMapsManager::publish_interactive_markers() auto add_controls = [](visualization_msgs::msg::InteractiveMarker & marker) { visualization_msgs::msg::InteractiveMarkerControl control; - // Move along X + // Move along X control.orientation.w = 1.0; control.orientation.x = 1.0; control.orientation.y = 0.0; @@ -572,19 +579,19 @@ void RoutesMapsManager::publish_interactive_markers() visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; marker.controls.push_back(control); - // Move along Y + // 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 + // 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 + // Rotate around Z (yaw), orientation as in interactive_markers examples control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; control.orientation.w = 1.0; @@ -594,7 +601,7 @@ void RoutesMapsManager::publish_interactive_markers() control.name = "rotate_z"; marker.controls.push_back(control); - // Button control to add a new segment starting from this endpoint + // 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 = @@ -603,7 +610,7 @@ void RoutesMapsManager::publish_interactive_markers() visualization_msgs::msg::Marker add_marker; add_marker.type = visualization_msgs::msg::Marker::SPHERE; - // Visual sphere for the add control + // Visual sphere for the add control add_marker.scale.x = 0.2; add_marker.scale.y = 0.2; add_marker.scale.z = 0.2; @@ -612,7 +619,7 @@ void RoutesMapsManager::publish_interactive_markers() add_marker.color.b = 0.0f; add_marker.color.a = 0.9f; - // Text label for the add control + // 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"; @@ -628,7 +635,7 @@ void RoutesMapsManager::publish_interactive_markers() marker.controls.push_back(add_ctrl); - // Button control to remove the segment this endpoint belongs to + // 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 = @@ -637,8 +644,8 @@ void RoutesMapsManager::publish_interactive_markers() 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. + // 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; @@ -648,7 +655,7 @@ void RoutesMapsManager::publish_interactive_markers() remove_marker.color.b = 0.0f; remove_marker.color.a = 0.9f; - // Text label for the remove control + // 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"; 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 c6ba97e4..b512cb3c 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 @@ -101,7 +101,8 @@ RoutesCostmapFilter::update(NavState & nav_state) std::vector segments; segments.reserve(routes.size()); for (const auto & seg : routes) { - segments.push_back({ + segments.push_back( + { seg.start.position.x, seg.start.position.y, seg.end.position.x, 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 889b8e7c..9655d56b 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 @@ -78,7 +78,8 @@ TEST_F(RoutesMapsManagerTest, LoadsRoutesFromValidYaml) node->declare_parameter("routes.package", std::string("")); node->declare_parameter("routes.map_path_file", std::string("")); - node->set_parameters({ + node->set_parameters( + { rclcpp::Parameter("routes.map_path_file", filename), rclcpp::Parameter("routes.package", std::string("")) }); @@ -106,7 +107,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenMapPathEmpty) node->declare_parameter("routes.package", std::string("")); node->declare_parameter("routes.map_path_file", std::string("")); - node->set_parameters({ + node->set_parameters( + { rclcpp::Parameter("routes.map_path_file", std::string("")), rclcpp::Parameter("routes.package", std::string("")) }); @@ -132,7 +134,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenYamlMissing) node->declare_parameter("routes.package", std::string("")); node->declare_parameter("routes.map_path_file", std::string("/tmp/non_existent_routes.yaml")); - node->set_parameters({ + node->set_parameters( + { rclcpp::Parameter("routes.map_path_file", std::string("/tmp/non_existent_routes.yaml")), rclcpp::Parameter("routes.package", std::string("")) }); @@ -159,7 +162,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenNoRoutesKey) node->declare_parameter("routes.package", std::string("")); node->declare_parameter("routes.map_path_file", std::string("")); - node->set_parameters({ + node->set_parameters( + { rclcpp::Parameter("routes.map_path_file", filename), rclcpp::Parameter("routes.package", std::string("")) }); @@ -191,7 +195,8 @@ TEST_F(RoutesMapsManagerTest, UpdateWritesRoutesIntoNavState) node->declare_parameter("routes.package", std::string("")); node->declare_parameter("routes.map_path_file", std::string("")); - node->set_parameters({ + node->set_parameters( + { rclcpp::Parameter("routes.map_path_file", filename), rclcpp::Parameter("routes.package", std::string("")) }); 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 e5e455c6..219b34b7 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 @@ -64,7 +64,7 @@ SimpleMapsManager::on_initialize() try { pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + "/" + map_path_file; - } catch(ament_index_cpp::PackageNotFoundError & ex) { + } catch (ament_index_cpp::PackageNotFoundError & ex) { throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } @@ -78,7 +78,8 @@ SimpleMapsManager::on_initialize() rclcpp::QoS(1).transient_local().reliable()); dynamic_occ_pub_ = node->create_publisher( - node->get_node_base_interface()->get_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + node->get_node_base_interface()->get_name() + std::string( + "/") + plugin_name + "/dynamic_map", 100); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); 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 5fbe317b..0287e0c3 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 @@ -155,7 +155,8 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) EXPECT_EQ(map.at(1, 1), 0); } -class FriendSimpleMapsManager : public easynav::SimpleMapsManager { +class FriendSimpleMapsManager : public easynav::SimpleMapsManager +{ public: void force_path(const std::string & path) {map_path_ = path;} }; 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 45a63c76..796c0ba6 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -91,7 +91,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, + 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)); @@ -159,7 +160,8 @@ void CostmapPlanner::update(NavState & nav_state) if (rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { latest_stamp = rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()); } - if (rclcpp::Time(goals.goals.front().header.stamp, + if (rclcpp::Time( + goals.goals.front().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { latest_stamp = rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()); @@ -167,15 +169,17 @@ void CostmapPlanner::update(NavState & nav_state) 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()); + RCLCPP_WARN( + get_node()->get_logger(), "Goals frame is not 'map': %s", + goals.header.frame_id.c_str()); return; } unsigned int gx, gy; if (!map.worldToMap(goal.position.x, goal.position.y, gx, gy)) { - RCLCPP_WARN(get_node()->get_logger(), "Goal (%.2f, %.2f) is outside the map", goal.position.x, - goal.position.y); + RCLCPP_WARN( + get_node()->get_logger(), "Goal (%.2f, %.2f) is outside the map", goal.position.x, + goal.position.y); return; } @@ -196,7 +200,8 @@ 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, + 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) && @@ -276,8 +281,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()); - const double initial_h = heuristic(static_cast(sx), static_cast(sy), - static_cast(gx), static_cast(gy)) * heuristic_scale_; + 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; 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 c18c8e70..eaadb896 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -132,7 +132,8 @@ SimplePlanner::update(NavState & nav_state) const auto clock_type = get_node()->get_clock()->get_clock_type(); rclcpp::Time latest_stamp(robot_pose.header.stamp, clock_type); - if (rclcpp::Time(goals.goals.front().header.stamp, + if (rclcpp::Time( + goals.goals.front().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { latest_stamp = rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()); @@ -141,13 +142,15 @@ SimplePlanner::update(NavState & nav_state) auto downsampled_map = map_typed.downsample(0.2); if (goals.header.frame_id != tf_info.map_frame) { - RCLCPP_WARN(get_node()->get_logger(), + RCLCPP_WARN( + get_node()->get_logger(), "SimplePlanner::update goals frame is not map (%s)", goals.header.frame_id.c_str()); return; } if (!downsampled_map->check_bounds_metric(goal.position.x, goal.position.y)) { - RCLCPP_WARN(get_node()->get_logger(), + RCLCPP_WARN( + get_node()->get_logger(), "SimplePlanner::update goal (%lf, %lf) outside the map", goal.position.x, goal.position.y); return; } @@ -210,7 +213,8 @@ SimplePlanner::a_star_path( double resolution) { RCLCPP_DEBUG(get_node()->get_logger(), "Running A* ============"); - RCLCPP_DEBUG(get_node()->get_logger(), "Path from (%lf m, %lf m) -> (%lf m, %lf m)", + RCLCPP_DEBUG( + get_node()->get_logger(), "Path from (%lf m, %lf m) -> (%lf m, %lf m)", start.position.x, start.position.y, goal.position.x, goal.position.y); @@ -220,7 +224,8 @@ SimplePlanner::a_star_path( auto [sx, sy] = map.metric_to_cell(start.position.x, start.position.y); auto [gx, gy] = map.metric_to_cell(goal.position.x, goal.position.y); - RCLCPP_DEBUG(get_node()->get_logger(), "Path from (%d, %d) -> (%d, %d)", + RCLCPP_DEBUG( + get_node()->get_logger(), "Path from (%d, %d) -> (%d, %d)", sx, sy, gx, gy); std::priority_queue, std::greater> open; @@ -271,7 +276,8 @@ SimplePlanner::a_star_path( path.push_back(pose); - RCLCPP_DEBUG(get_node()->get_logger(), "\t(%d, %d) = (%lf m, %lf m)", + RCLCPP_DEBUG( + get_node()->get_logger(), "\t(%d, %d) = (%lf m, %lf m)", cx, cy, px, py); std::tie(cx, cy) = came_from[idx(cx, cy)]; From c22a9efe90ddef0d443cd240f4e704fa732805aa Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 31 Jul 2026 22:14:23 +0200 Subject: [PATCH 225/230] thirdparty repos were modified Signed-off-by: Juan S. Cely G. --- .github/thirdparty.repos | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index 6feffa6f..16256309 100644 --- a/.github/thirdparty.repos +++ b/.github/thirdparty.repos @@ -1,13 +1,13 @@ repositories: ThirdParty/EasyNavigation: type: git - url: https://github.com/EasyNavigation/EasyNavigation.git - version: jazzy + url: https://github.com/juanscelyg/EasyNavigation.git + version: humble ThirdParty/yaets: type: git url: https://github.com/fmrico/yaets.git - version: jazzy + version: humble ThirdParty/NavMap: type: git - url: https://github.com/EasyNavigation/NavMap.git - version: jazzy + url: https://github.com/juanscelyg/NavMap.git + version: humble From 33228d33d93924f5b2aa6222c23610ee15748f65 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 31 Jul 2026 23:35:33 +0200 Subject: [PATCH 226/230] CI was updated Signed-off-by: Juan S. Cely G. --- .github/workflows/humble.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 246b9907..56613b43 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.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 easynav_mpc_controller target-ros2-distro: humble vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false From c453ebe26f51486fb95ed743de7e3a53393e11bc Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 31 Jul 2026 23:38:10 +0200 Subject: [PATCH 227/230] CI was updated Signed-off-by: Juan S. Cely G. --- .github/workflows/humble.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 56613b43..666ed7ec 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.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_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 + 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: humble vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false From 8cd4a288112527157d308fcd1d8645909595a432 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Sat, 1 Aug 2026 00:02:17 +0200 Subject: [PATCH 228/230] CI was updated Signed-off-by: Juan S. Cely G. --- .github/workflows/humble.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 666ed7ec..91b03bc4 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.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 easynav_mpc_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 easynav_mpc_controller target-ros2-distro: humble vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false From 1748a6ecb4567def5941358f1a1e6487e5e887da Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Sat, 1 Aug 2026 00:19:26 +0200 Subject: [PATCH 229/230] Workflow was modified Signed-off-by: Juan S. Cely G. --- .github/workflows/humble.yaml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 91b03bc4..d118920f 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -16,16 +16,14 @@ jobs: steps: - name: Repo checkout uses: actions/checkout@v4 - with: - ref: humble - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_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_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_fusion_localizer easynav_gps_localizer easynav_mpc_controller easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_octomap_maps_manager easynav_regulated_pp_controller easynav_routes_maps_manager 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 + vcs-repo-file-url: ${{ github.workspace }}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { From d5fca81c07f79c905fcbee4dff09b6959ac97b4d Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Sat, 1 Aug 2026 00:23:11 +0200 Subject: [PATCH 230/230] Workflow was modified Signed-off-by: Juan S. Cely G. --- .github/workflows/humble.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index d118920f..f805b91e 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -21,7 +21,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_fusion_localizer easynav_gps_localizer easynav_mpc_controller easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_octomap_maps_manager easynav_regulated_pp_controller easynav_routes_maps_manager 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_mpc_controller easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_octomap_maps_manager easynav_regulated_pp_controller easynav_routes_maps_manager easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller target-ros2-distro: humble vcs-repo-file-url: ${{ github.workspace }}/.github/thirdparty.repos skip-tests: false