WIP: Easynav recovery

[fmrico]

Hi,

Related to: EasyNavigation/EasyNavigation#119

This PR is a joint effort from @estherag and me to have a Recovery System for EasyNav. It is a Wok in Progress, so more work have to be done in this PR.

Best

[Copilot]

🟡 Changes recommended

Critical safety and correctness issues remain unresolved in perception handling, collision checking, and recovery state management.

Get a fresh assessment by requesting another Copilot review.

Pull request overview

This WIP adds recovery evaluators and mitigation plugins, AMCL recovery support, stale-path handling, and controller obstacle/collision features.

Changes:

• Adds recovery mitigations and diagnostic evaluators with tests and plugin registration.
• Adds AMCL convergence/relocalization behavior and planner path invalidation.
• Extends obstacle regulation and MPC collision checking.

File summaries

File	Description
recovery_mitigations/easynav_safe_retreat_recovery/tests/safe_retreat_recovery_tests.cpp	Tests safe-retreat recovery.
recovery_mitigations/easynav_safe_retreat_recovery/tests/CMakeLists.txt	Builds safe-retreat tests.
recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp	Implements safe-retreat recovery.
recovery_mitigations/easynav_safe_retreat_recovery/package.xml	Declares package metadata and dependencies.
recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp	Declares the safe-retreat API.
recovery_mitigations/easynav_safe_retreat_recovery/easynav_safe_retreat_recovery_plugins.xml	Registers the safe-retreat plugin.
recovery_mitigations/easynav_safe_retreat_recovery/CMakeLists.txt	Builds the safe-retreat package.
recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp	Tests human-assistance recovery.
recovery_mitigations/easynav_human_assistance_recovery/tests/CMakeLists.txt	Builds human-assistance tests.
recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp	Implements human-assistance recovery.
recovery_mitigations/easynav_human_assistance_recovery/package.xml	Declares package metadata and dependencies.
recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp	Declares the human-assistance API.
recovery_mitigations/easynav_human_assistance_recovery/easynav_human_assistance_recovery_plugins.xml	Registers the human-assistance plugin.
recovery_mitigations/easynav_human_assistance_recovery/CMakeLists.txt	Builds the human-assistance package.
recovery_mitigations/easynav_cancel_mission_recovery/tests/CMakeLists.txt	Builds mission-cancellation tests.
recovery_mitigations/easynav_cancel_mission_recovery/tests/cancel_mission_recovery_tests.cpp	Tests mission cancellation.
recovery_mitigations/easynav_cancel_mission_recovery/src/easynav_cancel_mission_recovery/CancelMissionRecovery.cpp	Implements mission cancellation.
recovery_mitigations/easynav_cancel_mission_recovery/package.xml	Declares package metadata and dependencies.
recovery_mitigations/easynav_cancel_mission_recovery/include/easynav_cancel_mission_recovery/CancelMissionRecovery.hpp	Declares the cancellation API.
recovery_mitigations/easynav_cancel_mission_recovery/easynav_cancel_mission_recovery_plugins.xml	Registers the cancellation plugin.
recovery_mitigations/easynav_cancel_mission_recovery/CMakeLists.txt	Builds the cancellation package.
recovery_mitigations/easynav_advance_recovery/tests/CMakeLists.txt	Builds advance-recovery tests.
recovery_mitigations/easynav_advance_recovery/tests/advance_recovery_tests.cpp	Tests advance recovery.
recovery_mitigations/easynav_advance_recovery/src/easynav_advance_recovery/AdvanceRecovery.cpp	Implements advance recovery.
recovery_mitigations/easynav_advance_recovery/package.xml	Declares package metadata and dependencies.
recovery_mitigations/easynav_advance_recovery/include/easynav_advance_recovery/AdvanceRecovery.hpp	Declares the advance-recovery API.
recovery_mitigations/easynav_advance_recovery/easynav_advance_recovery_plugins.xml	Registers the advance plugin.
recovery_mitigations/easynav_advance_recovery/CMakeLists.txt	Builds the advance-recovery package.
recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp	Tests obstacle diagnostics.
recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/CMakeLists.txt	Builds obstacle-evaluator tests.
recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp	Detects nearby obstacles.
recovery_evaluators/easynav_obstacle_too_close_evaluator/package.xml	Declares package metadata and dependencies.
recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp	Declares obstacle diagnostics.
recovery_evaluators/easynav_obstacle_too_close_evaluator/easynav_obstacle_too_close_evaluator_plugins.xml	Registers the obstacle evaluator.
recovery_evaluators/easynav_obstacle_too_close_evaluator/CMakeLists.txt	Builds the obstacle evaluator.
recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp	Tests no-path diagnostics.
recovery_evaluators/easynav_no_path_evaluator/tests/CMakeLists.txt	Builds no-path tests.
recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp	Detects missing paths.
recovery_evaluators/easynav_no_path_evaluator/package.xml	Declares package metadata and dependencies.
recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp	Declares no-path diagnostics.
recovery_evaluators/easynav_no_path_evaluator/easynav_no_path_evaluator_plugins.xml	Registers the no-path evaluator.
recovery_evaluators/easynav_no_path_evaluator/CMakeLists.txt	Builds the no-path evaluator.
recovery_evaluators/easynav_controller_stuck_evaluator/tests/controller_stuck_evaluator_tests.cpp	Tests stuck detection.
recovery_evaluators/easynav_controller_stuck_evaluator/tests/CMakeLists.txt	Builds stuck-evaluator tests.
recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp	Detects controller stagnation.
recovery_evaluators/easynav_controller_stuck_evaluator/package.xml	Declares package metadata and dependencies.
recovery_evaluators/easynav_controller_stuck_evaluator/include/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.hpp	Declares stuck diagnostics.
recovery_evaluators/easynav_controller_stuck_evaluator/easynav_controller_stuck_evaluator_plugins.xml	Registers the stuck evaluator.
recovery_evaluators/easynav_controller_stuck_evaluator/CMakeLists.txt	Builds the stuck evaluator.
planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp	Clears stale paths and handles unreachable goals.
planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp	Declares planner path-clearing support.
localizers/easynav_costmap_localizer/tests/CMakeLists.txt	Builds AMCL recovery tests.
localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp	Tests AMCL relocalization.
localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp	Tests AMCL convergence detection.
localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp	Implements AMCL relocalization.
localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp	Publishes AMCL covariance data.
localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp	Detects AMCL divergence.
localizers/easynav_costmap_localizer/package.xml	Declares AMCL recovery dependencies.
localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp	Declares relocalization behavior.
localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp	Declares the convergence evaluator.
localizers/easynav_costmap_localizer/easynav_costmap_localizer_plugins.xml	Registers AMCL recovery plugins.
localizers/easynav_costmap_localizer/CMakeLists.txt	Builds the AMCL recovery components.
controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp	Adds VFF obstacle configuration fields.
controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp	Applies obstacle-based velocity regulation.
controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp	Declares obstacle-regulation parameters.
controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp	Applies MPC collision-checking configuration.
controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp	Declares collision-checking state.

Review details

Suppressed comments (13)

controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp:47

• The new public parameter key is misspelled as colision_checker.active in both declaration and lookup. A normal configuration using collision_checker.active will be ignored and the collision checker will remain disabled, while the typo becomes the externally visible API; use the correctly spelled key consistently (and document it).

  node->declare_parameter<bool>(
    plugin_name + ".colision_checker.active", collision_checker_active_);

controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp:290

• The newly enabled collision check reduces the entire point set to its centroid before comparing with safety_radius_. A single obstacle point inside the safety radius can be diluted by many distant points, so the centroid stays outside the radius and the controller keeps the unsafe command; collision checking should use the nearest valid point (or reject the command if any point violates the radius), not the average position.

  if (collision_checker_active_) {
    collision_checker(&params, u);
  }

controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp:102

• These four public parameters are read from configuration, but the controller README's parameter table has no entries for robot_radius, safety_margin, z_min_filter, or robot_height. Add their defaults, units, and filtering/geometry semantics so deployments can configure the new obstacle-regulation behavior correctly.

  declare_and_get("robot_radius", robot_radius_);
  declare_and_get("safety_margin", safety_margin_);
  declare_and_get("z_min_filter", z_min_filter_);
  declare_and_get("robot_height", robot_height_);

controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp:96

• This new obstacle-regulation option also applies when allow_reversing selects negative X motion, but computeMinObstacleDistance() only filters points with x >= 0. An obstacle behind the robot—the reverse travel direction—is then ignored, the helper can return infinity, and the controller will not slow its reverse command. Make the filter depend on the selected travel direction before enabling this for reversing.

  declare_and_get(
    "use_obstacle_regulated_linear_velocity_scaling",
    use_obstacle_regulated_linear_velocity_scaling_);

planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp:258

• When A* finds no route, this clears the path but leaves the event-based replan gate above able to treat the empty path as a fresh successful plan. With continuous_replan_ == false, the next cycle can satisfy goals_ts < current_path_.header.stamp for the same goal and return before retrying; a later map update can therefore never recover the plan. Make the skip condition apply only when a non-empty path exists (or otherwise invalidate the failed-plan timestamp).

    clear_current_path(nav_state);

recovery_evaluators/easynav_controller_stuck_evaluator/include/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.hpp:21

• This public header uses std::pair inside reference_position_ but does not include <utility>. It currently relies on transitive standard-library includes; add the defining header directly for a self-contained header.

#include <optional>

recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp:115

• This no-command path preserves the previous progress timer. After a controller stops publishing cmd_vel and later resumes, the idle interval is counted as stuck even though no motion was requested; clear the progress reference before returning.

  if (!nav_state.has("cmd_vel")) {
    status.message = "no cmd_vel yet";
    publish_diagnostic(nav_state, status);
    return;

recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp:126

• This below-threshold command path also preserves the previous progress timer. A later non-zero command can inherit an old reference_time_ and report a false stuck episode; reset the progress reference when the robot is not currently commanded to move.

  if (commanded_speed < linear_velocity_threshold_) {
    status.message = "not commanded to move";
    publish_diagnostic(nav_state, status);
    return;

recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp:132

• When robot_pose is unavailable, this evaluator skips assessment but retains the old progress reference. Once pose data returns, elapsed time from before the data gap can trigger a false controller_stuck; reset the reference on this path.

  if (!nav_state.has("robot_pose")) {
    status.message = "no robot_pose yet";
    publish_diagnostic(nav_state, status);
    return;

recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp:95

• While a safety reflex is intervening, this branch leaves the stuck timer running. After a prolonged reflex stop clears, the evaluator can immediately report controller_stuck even though the lack of motion was caused by the safety reflex; re-arm or reset the progress reference on this path.

      reflex_status.level != diagnostic_msgs::msg::DiagnosticStatus::OK)

recovery_evaluators/easynav_obstacle_too_close_evaluator/package.xml:21

• The test source directly includes and constructs rclcpp_lifecycle::LifecycleNode, but this manifest does not declare rclcpp_lifecycle as a test dependency. Add the direct dependency instead of relying on a transitive export from easynav_core.
  recovery_mitigations/easynav_advance_recovery/include/easynav_advance_recovery/AdvanceRecovery.hpp:21
• This public header declares std::pair in start_position_ but does not include <utility>. It currently depends on transitive standard-library includes from other headers; include the defining header directly so the plugin remains self-contained across toolchains.
  recovery_mitigations/easynav_human_assistance_recovery/package.xml:21
• The test source directly includes geometry_msgs/msg/twist_stamped.hpp, but geometry_msgs is not declared in this package's dependencies. Add it as a test dependency so the test does not rely on a transitive export.

• Files reviewed: 67/67 changed files
• Comments generated: 11
• Review effort level: Lite

---

💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.