Easynav recovery

[fmrico]

Hi,

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.

EasyNav's recovery system: a two-level architecture that detects and reacts to
navigation failures, combining Nav2's lightweight plugin mechanics with the
evaluation/mitigation separation and mission-level escalation proposed by TOMASys/SysSelf,
without requiring OWL/DL reasoning.

• Level 0 (real-time safety reflexes): a SafetyReflexBase plugin checked on every RT
  cycle, right before cmd_vel is published, regardless of who produced it.
• Level 1 (deliberative recovery): a new RecoveryManagerNode that runs
  RecoveryEvaluatorBase diagnosis plugins and arbitrates between RecoveryMitigationBase
  plugins, handing off cmd_vel ownership via a control_owner key when a mitigation needs
  to move the robot.

Full design + as-built documentation: docs/recoveries_easynav.md.

Demo

Architecture

flowchart TB
    subgraph proc["easynav_system process (single process, shared NavState)"]
        SEN[SensorsNode] --> NS[(NavState)]
        LOC[LocalizerNode] --> NS
        MAP[MapsManagerNode] --> NS
        PLN[PlannerNode] --> NS
        CTR["ControllerNode\n(active when control_owner=controller)"] --> CVEL[(candidate cmd_vel)]
        MIT --> CVEL

        subgraph rt["Level 0 - SystemNode RT cycle"]
            CVEL --> REFLEX["SafetyReflexBase plugins\n(CollisionSafetyReflex)"]
            REFLEX -->|final cmd_vel| PUB[/publish cmd_vel/]
        end
        REFLEX -.diagnostics.-> DIAG

        NS --> EV1[Evaluator plugin A]
        NS --> EV2[Evaluator plugin B]
        NS --> EVn[Evaluator plugin N]
        EV1 --> DIAG[("NavState 'diagnostics' group")]
        EV2 --> DIAG
        EVn --> DIAG

        DIAG --> RM["Level 1 - RecoveryManagerNode\nselection + arbitration (non-RT)"]
        RM --> MIT[Active mitigation plugin]
        RM -->|writes control_owner| NS
        RM -->|publishes| PUBDIAG[/diagnostics/]
        RM -->|publishes| PUBMIT[mitigation]
        MIT -.mission_cancel_requested.-> GM[GoalManager]
    end

Loading

What's included

• SafetyReflexBase interface (level 0, RT) + CollisionSafetyReflex reference plugin, replacing the old collision check that lived in ControllerMethodBase
• RecoveryEvaluatorBase interface (level 1, read-only diagnosis)
• RecoveryMitigationBase interface (level 1, acts), with priority + exclusion arbitration (no cooldown/retry table)
• New easynav_recovery package: RecoveryManagerNode (lifecycle node recovery_node), wired into SystemNode's RT and non-RT cycles
• control_owner handoff mechanism for movement mitigations to take over cmd_vel
• Generic evaluators: NoPathEvaluator, ControllerStuckEvaluator, ObstacleTooCloseEvaluator
• Generic mitigations: SafeRetreatRecovery, AdvanceRecovery, HumanAssistanceRecovery, CancelMissionRecovery
• Component-specialized recovery example: AmclConvergenceEvaluator / AmclRelocalizeMitigation co-located in easynav_costmap_localizer
• Mission-level escalation: GoalManager::set_error() wired to a real path via the mission_cancel_requested signal
• Exception safety at every plugin invocation point (controller/planner/localizer/maps_manager/recovery), so a misbehaving plugin can't crash the process
• Observability: real /diagnostics (DiagnosticArray), a "mitigation" narrative topic, and TUI panels for both
• Unit tests for RecoveryManagerNode and every new evaluator/mitigation plugin
• Design and as-built documentation (docs/recoveries_easynav.md)

Example of parameters:

These are the parameters used in the video above.

recovery_node:
  ros__parameters:
    use_sim_time: true
    evaluator_types: [no_path, obstacle_close, amcl_convergence, controller_stuck]
    no_path:
      plugin: easynav_no_path_evaluator/NoPathEvaluator
    obstacle_close:
      plugin: easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator
      safe_distance: 0.4
    controller_stuck:
      plugin: easynav_controller_stuck_evaluator/ControllerStuckEvaluator
    amcl_convergence:
      plugin: easynav_costmap_localizer/AmclConvergenceEvaluator
      covariance_threshold: 1.0
      timeout: 7.0
      rotation_speed: 0.5
    mitigation_types: [retreat, amcl_relocalize, advance, human_assistance, cancel_mission]
    retreat:
      plugin: easynav_safe_retreat_recovery/SafeRetreatRecovery
    advance:
      plugin: easynav_advance_recovery/AdvanceRecovery
      priority: 10
      advance_distance: 0.3
      advance_speed: 0.1
      escalate_after: 15.0
    amcl_relocalize:
      plugin: easynav_costmap_localizer/AmclRelocalizeMitigation
      priority: 10
      rotation_speed: 0.3
      timeout: 5.0
      covariance_threshold: 1.0
    human_assistance:
      plugin: easynav_human_assistance_recovery/HumanAssistanceRecovery
      priority: 1000
      timeout: 30.0
    cancel_mission:
      plugin: easynav_cancel_mission_recovery/CancelMissionRecovery
      priority: 2000

[Copilot]

🟡 Changes recommended

Unresolved critical concurrency and exception-safety defects, plus plugin and dependency issues, block approval.

Get a fresh assessment by requesting another Copilot review.

Pull request overview

Adds EasyNav’s two-level recovery architecture with real-time safety reflexes, non-RT recovery orchestration, control ownership, diagnostics, mission escalation, tests, and documentation.

Changes:

• Adds recovery/reflex interfaces, plugin loading, arbitration, and lifecycle integration.
• Moves collision handling into a safety-reflex plugin.
• Adds TUI diagnostics, packaging updates, tests, and recovery documentation.

File summaries

File	Reviewed change and final findings
easynav/package.xml	Adds the recovery dependency. Moderate (2 votes): recovery is not included in the metapackage’s exported CMake dependencies.
easynav_tools/package.xml	Adds tool dependencies. Critical (3 votes): diagnostic_msgs is imported but not declared directly.
easynav_tools/easynav_tools/tui/app.py	Adds diagnostics and mitigation panels.
easynav_tools/easynav_tools/controller/ros_controllers.py	Adds diagnostic message handling. Moderate (3 votes): all DEBUG logs are treated as the manager marker, hiding legitimate reports.
easynav_system/tests/system_safety_reflex_tests.cpp	Tests safety-reflex loading.
easynav_system/tests/CMakeLists.txt	Registers safety-reflex tests.
easynav_system/src/easynav_system/SystemNode.cpp	Integrates recovery and RT reflex cycles. Critical (3 votes): control_owner access can race between RT and non-RT threads. Moderate (1 vote): destructor may read undeclared safety_reflex_types.
easynav_system/src/easynav_system/GoalManager.cpp	Handles mission cancellation escalation. Moderate (2 votes): diagnostic has() access is unsynchronized.
easynav_system/package.xml	Adds recovery and diagnostics dependencies.
easynav_system/include/easynav_system/SystemNode.hpp	Declares recovery and reflex members.
easynav_system/include/easynav_system/GoalManager.hpp	Adjusts the height tolerance default.
easynav_system/CMakeLists.txt	Builds and links recovery components. Moderate (1 vote): diagnostic_msgs is not linked or exported directly.
easynav_sensors/src/easynav_sensors/types/PointPerception.cpp	Throttles TF failure logging.
easynav_recovery/tests/recovery_manager_node_tests.cpp	Tests recovery manager behavior.
easynav_recovery/tests/CMakeLists.txt	Registers recovery tests.
easynav_recovery/src/easynav_recovery/RecoveryManagerNode.cpp	Implements recovery orchestration. Critical (3 votes): RT/non-RT arbitration state is unsynchronized. Critical (3 votes): can_handle() is not exception-safe. Critical (2 votes): requires_control() can throw on the RT path. Critical (2 votes): diagnostic has() access races with updates. Moderate (2 votes): completed non-control mitigations can be repeatedly selected. Moderate (1 vote): control mitigation success is reported without validating a fresh command. Moderate (1 vote): destructor can read undeclared mitigation_types or evaluator_types. Moderate (1 vote): mitigation report lookup is unsynchronized.
easynav_recovery/src/easynav_recovery/DummyMitigation.cpp	Adds a test mitigation plugin.
easynav_recovery/src/easynav_recovery/DummyEvaluator.cpp	Adds a test evaluator plugin.
easynav_recovery/package.xml	Defines recovery package dependencies.
easynav_recovery/include/easynav_recovery/RecoveryManagerNode.hpp	Declares the recovery manager API and state.
easynav_recovery/include/easynav_recovery/DummyMitigation.hpp	Declares the dummy mitigation.
easynav_recovery/include/easynav_recovery/DummyEvaluator.hpp	Declares the dummy evaluator.
easynav_recovery/easynav_recovery_plugins.xml	Registers recovery plugins. Moderate (1 vote each): advertised evaluator and mitigation catalogs are not present or registered.
easynav_recovery/CMakeLists.txt	Builds and exports recovery libraries.
easynav_core/test/obstacle_proximity_tests.cpp	Tests obstacle proximity calculations.
easynav_core/test/core_method_test.cpp	Tests recovery interfaces and exception handling.
easynav_core/test/CMakeLists.txt	Registers core tests.
easynav_core/src/easynav_core/SafetyReflexBase.cpp	Implements safety-reflex handling. Critical (1 vote): diagnostics read-modify-write operations can lose reflex entries.
easynav_core/src/easynav_core/RecoveryMitigationBase.cpp	Implements mitigation lifecycle and reporting. Critical (1 vote): failed on_start() can still leave a mitigation active and take control ownership.
easynav_core/src/easynav_core/RecoveryEvaluatorBase.cpp	Implements evaluator execution and diagnostics.
easynav_core/src/easynav_core/PlannerMethodBase.cpp	Adds planner exception handling.
easynav_core/src/easynav_core/ObstacleProximity.cpp	Implements proximity computation. Critical (1 vote): perception fusion can mutate shared RT state and race sensor access.
easynav_core/src/easynav_core/MapsManagerBase.cpp	Adds map-manager exception handling.
easynav_core/src/easynav_core/LocalizerMethodBase.cpp	Adds localizer exception handling.
easynav_core/src/easynav_core/ControllerMethodBase.cpp	Removes embedded collision handling and adds exception handling.
easynav_core/package.xml	Updates core dependencies.
easynav_core/include/easynav_core/SafetyReflexBase.hpp	Defines the reflex interface.
easynav_core/include/easynav_core/RecoveryMitigationBase.hpp	Defines the mitigation interface.
easynav_core/include/easynav_core/RecoveryEvaluatorBase.hpp	Defines the evaluator interface.
easynav_core/include/easynav_core/ObstacleProximity.hpp	Declares the proximity API.
easynav_core/include/easynav_core/ControllerMethodBase.hpp	Removes collision-specific API.
easynav_core/CMakeLists.txt	Builds new core recovery components.
easynav_controller/src/easynav_controller/CollisionSafetyReflex.cpp	Implements the collision safety plugin.
easynav_controller/package.xml	Adds reflex dependencies.
easynav_controller/include/easynav_controller/CollisionSafetyReflex.hpp	Declares the collision reflex.
easynav_controller/easynav_safety_reflexes_plugins.xml	Registers the collision reflex.
easynav_controller/CMakeLists.txt	Builds and exports the reflex plugin.
easynav_common/tests/navstate_tests.cpp	Tests NavState grouping and ordering.
easynav_common/include/easynav_common/types/NavState.hpp	Adds grouped key access and sorted debugging.
docs/recoveries_easynav.md	Documents recovery architecture and deployment. Nit (2 votes): documentation advertises evaluator, mitigation, and AMCL plugins that are not included or registered.

Review details

Suppressed comments (11)

easynav_core/src/easynav_core/RecoveryMitigationBase.cpp:110

• Control-owning mitigations are executed from cycle_rt(), and their on_cycle() implementations are allowed to call report(). This path performs synchronous ROS logging and constructs/copies a MitigationReport before writing it into NavState, all of which can allocate or block in the RT loop. Queue only RT-safe data and publish/log it from the non-RT cycle, or explicitly prohibit report() from RT mitigations.

  switch (level) {
    case rcl_interfaces::msg::Log::DEBUG:
      RCLCPP_DEBUG(node->get_logger(), "%s", msg.c_str());
      break;
    case rcl_interfaces::msg::Log::WARN:

easynav_recovery/easynav_recovery_plugins.xml:5

• This manifest registers only the dummy evaluator, while the PR description and deployment example advertise NoPathEvaluator, ControllerStuckEvaluator, ObstacleTooCloseEvaluator, and the AMCL-specific evaluator. Those implementations/manifests are not present in this checkout, so the documented evaluator configuration cannot load the claimed catalog; either include/register it or correct the description and docs.

  <library path="dummy_evaluator">
    <class name="easynav_recovery/DummyEvaluator" type="easynav::DummyEvaluator" base_class_type="easynav::RecoveryEvaluatorBase">
      <description>
      A default "dummy" implementation for RecoveryEvaluatorBase. Always reports OK.

easynav_recovery/easynav_recovery_plugins.xml:14

• This manifest registers only DummyMitigation, while the PR description and deployment example advertise four generic mitigations plus AmclRelocalizeMitigation. No corresponding mitigation plugin is registered here or present in this checkout, so the documented mitigation_types configuration cannot load those recovery actions; include/register them or correct the claimed scope.

  <library path="dummy_mitigation">
    <class name="easynav_recovery/DummyMitigation" type="easynav::DummyMitigation" base_class_type="easynav::RecoveryMitigationBase">
      <description>
      A default "dummy" implementation for RecoveryMitigationBase. Accepts any non-OK
      diagnostic and immediately reports SUCCEEDED. Serves as an example and a reference plugin.

easynav_recovery/src/easynav_recovery/RecoveryManagerNode.cpp:378

• NavState::get() returns a reference after releasing state_mutex_, while SafetyReflexBase can update this diagnostic from the RT thread. status can therefore be read while the RT thread overwrites it, causing a data race during can_handle(); use get_safe<DiagnosticStatus>() into a local value here, as publish_diagnostics() does above.
  easynav_recovery/src/easynav_recovery/RecoveryManagerNode.cpp:359
• The RT completion path has the same reselection problem: after a control-owning mitigation returns SUCCEEDED, it is reset while its triggering diagnostic can still be ERROR, so the next non-RT cycle selects it again. Keep a completed mitigation excluded for that diagnostic (or otherwise hold the episode) until the evaluator reports OK.
  easynav_recovery/src/easynav_recovery/RecoveryManagerNode.cpp:364
• The return value is documented as true only when the mitigation produced cmd_vel, but this is unconditional for any active control mitigation. A plugin can return SUCCEEDED or RUNNING without writing a command (DummyMitigation does), so SystemNode may publish a stale previous command or claim a write that never happened; track/validate a fresh command or make the interface enforce that contract before returning true.
  easynav_recovery/src/easynav_recovery/RecoveryManagerNode.cpp:105
• If evaluator loading fails before on_configure() reaches the mitigation section, mitigation_types is never declared. The destructor still calls get_parameter("mitigation_types", ...), so the failure path can throw again during destruction and terminate the test/process; guard this parameter read just as the plugin-specific reads are guarded.
  easynav_recovery/src/easynav_recovery/RecoveryManagerNode.cpp:89
• RecoveryManagerNode can be destroyed without ever being configured (the node_name test does exactly that), but this unconditional get_parameter() throws when evaluator_types is undeclared. That makes ordinary construction/destruction and early lifecycle failures terminate instead of cleaning up; guard the top-level parameter reads with has_parameter() (and do the same for the mitigation list).
  easynav_recovery/src/easynav_recovery/RecoveryManagerNode.cpp:312
• This existence check reads NavState::values_ through has() without the state mutex while RecoveryMitigationBase::report() may insert or update mitigation.pending_report from the RT loop. The subsequent get_safe() does not make the preceding check safe; use a thread-safe optional/snapshot lookup instead.
  easynav_system/CMakeLists.txt:23
• diagnostic_msgs is now used directly by GoalManager.cpp, but this CMake change only calls find_package; the message target is absent from target_link_libraries() and from ament_export_dependencies(). The build currently relies on easynav_recovery's transitive dependency, which is fragile and can leave downstream consumers without the direct include/link dependency. Add the target and export it explicitly.
  easynav_system/src/easynav_system/SystemNode.cpp:93
• If any managed child fails configuration before this point (for example, a bad recovery plugin), safety_reflex_types has never been declared on SystemNode. The destructor then calls get_parameter() on an undeclared parameter, which can throw during destruction and terminate the process instead of allowing the configuration failure to be reported; guard the read with has_parameter().

• Files reviewed: 50/50 changed files
• Comments generated: 14
• Review effort level: Lite

---

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