From 3492db69f58d7d44be17dd83c63160560622561a Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 4 Dec 2025 21:16:45 +0100 Subject: [PATCH 1/4] Collision avoidance was improved --- .../easynav_mpc_controller/MPCController.hpp | 16 ++++++---- .../easynav_mpc_controller/MPCController.cpp | 32 +++++++++++++------ 2 files changed, 33 insertions(+), 15 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index e01b6e6c..66edbf46 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -66,12 +66,16 @@ class MPCController : public ControllerMethodBase void collision_checker(void *data, std::vector & u); protected: - int horizon_steps_{5}; ///< Prediction horizon for MPC. - double dt_{0.1}; ///< Time step for MPC. - double safety_radius_{0.35}; ///< Safety radius to avoid obstacles - double max_lin_vel_{1.5}; ///< Maximum linear velocity for MPC. - double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. - bool verbose_{false}; ///< Value to debug on terminal + int horizon_steps_{5}; ///< Prediction horizon for MPC. + double dt_{0.1}; ///< Time step for MPC. + double safety_radius_{0.35}; ///< Safety radius to avoid obstacles + double max_lin_vel_{1.5}; ///< Maximum linear velocity for MPC. + double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. + bool verbose_{false}; ///< Value to debug on terminal + double last_v_{0.0}; ///< Last value for linear velocity before than collision + double last_w_{0.0}; ///< Last value for angular velocity before than collision + bool collision_state_{false}; ///< Collision state flag + double collision_factor_{0.618033}; ///< Collision avoidance for recalculate velocities // Fallback goal tolerances if GoalManager does not publish them double fallback_goal_pos_tol_{0.05}; ///< Default positional tolerance (meters). diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index ceee0ca1..a984b68c 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -107,14 +107,26 @@ MPCController::collision_checker(void *data, std::vector & u) real_points++; } } - x_m /= real_points; - y_m /= real_points; - dist = std::hypot(x_m, y_m); - angle = std::atan2(y_m, x_m) - params->theta0[2]; - if(real_points != 0 && dist < safety_radius_) { - std::cerr << "Detection at: " << dist << " Theta: " << angle << std::endl; - u[0] -= dist / real_points * dt_; - u[1] -= angle / real_points * dt_; + if(real_points != 0) { + x_m /= real_points; + y_m /= real_points; + dist = std::hypot(x_m, y_m); + angle = std::atan2(y_m, x_m) - params->theta0[2]; + if(dist < safety_radius_) { + if(!collision_state_) { + collision_state_ = true; + last_v_ = u[0]; + last_w_ = u[1]; + } + RCLCPP_WARN(get_node()->get_logger(), + "[COLLISION] Collision detected at: [%f] m and Theta: [%f] degrees", dist, (angle * 180.00 / M_PI) ); + last_v_ = collision_factor_ * last_v_ * safety_radius_ / dist; + last_w_ = collision_factor_ * last_w_ * safety_radius_ / dist; + u[0] = -last_v_; + u[1] = -last_w_; + } else { + collision_state_ = false; + } } } @@ -271,7 +283,9 @@ MPCController::update_rt(NavState & nav_state) std::cerr << "Optimization Error: " << e.what() << std::endl; } - collision_checker(¶ms, u); + if (collision_checker_active_) { + collision_checker(¶ms, u); + } // Final alignment phase with hysteresis on distance: // - Enter when dist_to_goal <= 0.5 * pos_tol From a07d155fa13d2faf1d8fbb611beb365bf2ff6527 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 19 Dec 2025 10:39:54 +0100 Subject: [PATCH 2/4] Collision checker scope was changed --- .../src/easynav_mpc_controller/MPCController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index a984b68c..73f28549 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -283,7 +283,7 @@ MPCController::update_rt(NavState & nav_state) std::cerr << "Optimization Error: " << e.what() << std::endl; } - if (collision_checker_active_) { + if (ControllerMethodBase::collision_checker_active_) { collision_checker(¶ms, u); } From 297d3a4d81ecf5d8c559884da77b92630857bbc7 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 17:07:45 +0100 Subject: [PATCH 3/4] Dependencies were corrected --- .../src/easynav_mpc_controller/MPCController.cpp | 3 ++- maps_managers/easynav_octomap_maps_manager/package.xml | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index d042407c..66ec8eea 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -119,7 +119,8 @@ MPCController::collision_checker(void *data, std::vector & u) last_w_ = u[1]; } RCLCPP_WARN(get_node()->get_logger(), - "[COLLISION] Collision detected at: [%f] m and Theta: [%f] degrees", dist, (angle * 180.00 / M_PI) ); + "[COLLISION] Collision detected at: [%f] m and Theta: [%f] degrees", dist, + (angle * 180.00 / M_PI) ); last_v_ = collision_factor_ * last_v_ * safety_radius_ / dist; last_w_ = collision_factor_ * last_w_ * safety_radius_ / dist; u[0] = -last_v_; diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 002b28db..33e837e0 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -18,7 +18,6 @@ nav_msgs sensor_msgs ament_index_cpp - octomap octomap_ros octomap_msgs pcl_conversions From f2e085c6cf146e2c5b8aaa0899208476b641cd7b Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 18 Feb 2026 10:19:49 +0100 Subject: [PATCH 4/4] CI Updated --- .github/workflows/jazzy.yaml | 3 +-- .github/workflows/jazzy_cron.yaml | 4 +--- .github/workflows/kilted.yaml | 3 +-- .github/workflows/kilted_cron.yaml | 4 +--- .github/workflows/rolling_cron.yaml | 2 +- 5 files changed, 5 insertions(+), 11 deletions(-) diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 11301a4d..365cdccf 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -24,9 +24,8 @@ jobs: - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml index ac2a5efd..c7049a98 100644 --- a/.github/workflows/jazzy_cron.yaml +++ b/.github/workflows/jazzy_cron.yaml @@ -18,10 +18,8 @@ jobs: - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy - ref: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 5eff233e..0a4f68ed 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -22,9 +22,8 @@ jobs: - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: kilted - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml index dc93850f..c4b6b82b 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.github/workflows/kilted_cron.yaml @@ -18,10 +18,8 @@ jobs: - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: kilted - ref: kilted - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml index 740df03b..ef59286d 100644 --- a/.github/workflows/rolling_cron.yaml +++ b/.github/workflows/rolling_cron.yaml @@ -18,7 +18,7 @@ jobs: - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: rolling ref: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos