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 01/13] 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 0000000..c875752 --- /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 0000000..7522dd4 --- /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 0000000..c5c47a8 --- /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 0000000..4c4e8d3 --- /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 0000000..381bc70 --- /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 0000000..b671d26 --- /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 0000000..f3a604d --- /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 0000000..dec983f --- /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 0000000..7c7c867 --- /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 0000000..a9bc6ee --- /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 02/13] 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 4c4e8d3..68259b8 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 b671d26..7902a91 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 f3a604d..c1609bc 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 dec983f..efdf1e9 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 03/13] 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 68259b8..607e207 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 381bc70..bb6cde6 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 7902a91..babc285 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 efdf1e9..7e3f644 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 a9bc6ee..467b146 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 2a7b1d142844705c4dcecf05f005e1882a9cedcd Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Mon, 6 Jul 2026 11:57:17 +0200 Subject: [PATCH 04/13] 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 d70f2b6..cafefba 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 05/13] 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 b522247..4ad4cfb 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 66be212..3f8507e 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 aa0d04e..2e9bdf4 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 e8e1a6c..c79fd50 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 3c0645f..d12884b 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 7675162..2681b6e 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 06/13] 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 d229c8c..033cc8b 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 6259a59..0ce41b2 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 2b6dc64..fe3b726 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 89dfed8..720c89f 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 d391cb4..3f1dbb1 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 36efd00..12dd163 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 07/13] 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 c4abe86..8fc6a74 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 08/13] 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 f6bfdf2..702f5be 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 09/13] 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 9a32f1f..52272e5 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 8a06b1b..428f59c 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 3f1dbb1..23729b9 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 3de9cd3..67e6c9a 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 3f8507e..0010a11 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 4df61c6..f03c72d 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 69e4167..d29fab1 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 19cf79d..d962bfd 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 4c55b8f..913234a 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 1bf0af5..3e1ba4f 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 7ea6563..975883b 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 10a2a89..e5fb7b2 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 a6f574c..011e462 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 10f59ab..0075561 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 2681b6e..2980211 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 e6adb41..449dee2 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 6750fca..df142bd 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 324dc63..efe694c 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 b869c68..aa8f707 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 10/13] 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 67e6c9a..7acf601 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 11/13] 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 d29fab1..dfce79f 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 12/13] 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 0010a11..404abe2 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 13/13] 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 404abe2..bfab979 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();