WIP: Easynav recovery

controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp

@@ -85,6 +85,7 @@ class MPCController : public ControllerMethodBase
   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
+  bool collision_checker_active_{false};  ///< Enables the in-loop obstacle constraint above
 
   // Fallback goal tolerances if GoalManager does not publish them
   double fallback_goal_pos_tol_{0.05};   ///< Default positional tolerance (meters).

controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp

@@ -43,6 +43,8 @@ MPCController::on_initialize()
 
   node->declare_parameter<double>(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_);
   node->declare_parameter<double>(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_);
+  node->declare_parameter<bool>(
+    plugin_name + ".colision_checker.active", collision_checker_active_);
 
   node->get_parameter<int>(plugin_name + ".horizon_steps", horizon_steps_);
   node->get_parameter<double>(plugin_name + ".dt", dt_);
@@ -53,6 +55,7 @@ MPCController::on_initialize()
 
   node->get_parameter<double>(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_);
   node->get_parameter<double>(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_);
+  node->get_parameter<bool>(plugin_name + ".colision_checker.active", collision_checker_active_);
 
   optimizer_ = std::make_unique<MPCOptimizer>();
 
@@ -282,7 +285,7 @@ MPCController::update_rt(NavState & nav_state)
     std::cerr << "Optimization Error: " << e.what() << std::endl;
   }
 
-  if (ControllerMethodBase::collision_checker_active_) {
+  if (collision_checker_active_) {
     collision_checker(&params, u);
   }
 

controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp

@@ -101,6 +101,13 @@ class RegulatedPurePursuitController : public ControllerMethodBase
   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.
 
+  // Robot geometry used by computeMinObstacleDistance() (own copy: this is a distinct
+  // speed-regulation heuristic, unrelated to the level-0 CollisionSafetyReflex).
+  double robot_radius_{0.35};   ///< Robot radius used when measuring obstacle distance (m).
+  double safety_margin_{0.1};   ///< Safety margin added to the robot radius (m).
+  double z_min_filter_{0.0};    ///< Minimum Z considered when filtering point clouds (m).
+  double robot_height_{0.5};    ///< Vertical extent of the robot used for filtering (m).
+
   // --- 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.

controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp

@@ -96,6 +96,10 @@ RegulatedPurePursuitController::on_initialize()
     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("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_);
 
   declare_and_get("min_approach_linear_velocity", min_approach_linear_velocity_);
   declare_and_get("approach_velocity_scaling_dist", approach_velocity_scaling_dist_);

controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp

@@ -20,6 +20,7 @@
 #define EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_
 
 #include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
 
 #include "easynav_core/ControllerMethodBase.hpp"
 

localizers/easynav_costmap_localizer/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
 find_package(Eigen3 REQUIRED NO_MODULE)
 
 
@@ -43,13 +44,43 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
   ${nav_msgs_TARGETS}
 )
 
+add_library(amcl_convergence_evaluator SHARED
+  src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp
+)
+target_include_directories(amcl_convergence_evaluator PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(amcl_convergence_evaluator PUBLIC
+  easynav_core::easynav_core
+  pluginlib::pluginlib
+  ${diagnostic_msgs_TARGETS}
+)
+
+add_library(amcl_relocalize_mitigation SHARED
+  src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp
+)
+target_include_directories(amcl_relocalize_mitigation PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(amcl_relocalize_mitigation PUBLIC
+  easynav_common::easynav_common
+  easynav_core::easynav_core
+  pluginlib::pluginlib
+  ${diagnostic_msgs_TARGETS}
+  ${geometry_msgs_TARGETS}
+)
+
 install(
   DIRECTORY include/
   DESTINATION include/${PROJECT_NAME}
 )
 
 install(TARGETS
   ${PROJECT_NAME}
+  amcl_convergence_evaluator
+  amcl_relocalize_mitigation
   EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -67,7 +98,11 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")
-ament_export_libraries(${PROJECT_NAME})
+ament_export_libraries(
+  ${PROJECT_NAME}
+  amcl_convergence_evaluator
+  amcl_relocalize_mitigation
+)
 ament_export_targets(export_${PROJECT_NAME})
 
 # Register the planning plugins
@@ -86,6 +121,7 @@ ament_export_dependencies(
   rclcpp
   geometry_msgs
   nav_msgs
+  diagnostic_msgs
   Eigen3
 )
 ament_package()

localizers/easynav_costmap_localizer/easynav_costmap_localizer_plugins.xml

@@ -6,4 +6,20 @@
       </description>
     </class>
   </library>
+  <library path="amcl_convergence_evaluator">
+    <class name="easynav_costmap_localizer/AmclConvergenceEvaluator" type="easynav::AmclConvergenceEvaluator" base_class_type="easynav::RecoveryEvaluatorBase">
+      <description>
+      Diagnoses AMCL particle-filter divergence from the pose covariance trace. See
+      docs/recoveries_easynav.md §5.9.
+      </description>
+    </class>
+  </library>
+  <library path="amcl_relocalize_mitigation">
+    <class name="easynav_costmap_localizer/AmclRelocalizeMitigation" type="easynav::AmclRelocalizeMitigation" base_class_type="easynav::RecoveryMitigationBase">
+      <description>
+      Rotates in place to help AMCL relocalize after a divergence diagnostic. See
+      docs/recoveries_easynav.md §5.9.
+      </description>
+    </class>
+  </library>
 </class_libraries>

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp

@@ -0,0 +1,60 @@
+// 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.
+// 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 Declaration of the AmclConvergenceEvaluator plugin.
+
+#ifndef EASYNAV_COSTMAP_LOCALIZER__AMCLCONVERGENCEEVALUATOR_HPP_
+#define EASYNAV_COSTMAP_LOCALIZER__AMCLCONVERGENCEEVALUATOR_HPP_
+
+#include "easynav_core/RecoveryEvaluatorBase.hpp"
+
+namespace easynav
+{
+
+/**
+ * @class AmclConvergenceEvaluator
+ * @brief Level-1 recovery evaluator: diagnoses AMCL particle-filter divergence.
+ *
+ * Lives in the same package as AMCLLocalizer instead of the generic recovery_evaluators
+ * catalog: only the author of the localizer plugin really knows that particle dispersion (here,
+ * the trace of the pose covariance AMCLLocalizer already computes) is a good indicator of lost
+ * convergence.
+ *
+ * Reads the fixed key "localizer.amcl.covariance_trace" (written by AMCLLocalizer from both
+ * its RT and non-RT cycles) and publishes `hardware_id = "localizer.amcl"`, matched by
+ * AmclRelocalizeMitigation in this same package/manifest — no compile-time dependency between
+ * the two, only this agreed-upon diagnostic vocabulary.
+ */
+class AmclConvergenceEvaluator : public easynav::RecoveryEvaluatorBase
+{
+public:
+  AmclConvergenceEvaluator() = default;
+  ~AmclConvergenceEvaluator() = default;
+
+  void on_initialize() override;
+
+protected:
+  void update(NavState & nav_state) override;
+
+private:
+  /// @brief Covariance trace (var_x + var_y + var_yaw) above which AMCL is considered
+  /// diverged. Starting point only — depends on sensor/robot and needs tuning on the real
+  /// platform.
+  double covariance_threshold_ {1.0};
+};
+
+}  // namespace easynav
+
+#endif  // EASYNAV_COSTMAP_LOCALIZER__AMCLCONVERGENCEEVALUATOR_HPP_

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp

@@ -0,0 +1,71 @@
+// 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.
+// 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 Declaration of the AmclRelocalizeMitigation plugin.
+
+#ifndef EASYNAV_COSTMAP_LOCALIZER__AMCLRELOCALIZEMITIGATION_HPP_
+#define EASYNAV_COSTMAP_LOCALIZER__AMCLRELOCALIZEMITIGATION_HPP_
+
+#include "rclcpp/time.hpp"
+
+#include "easynav_core/RecoveryMitigationBase.hpp"
+
+namespace easynav
+{
+
+/**
+ * @class AmclRelocalizeMitigation
+ * @brief Level-1 movement mitigation: rotates in place to help AMCL relocalize.
+ *
+ * Selected for diagnostics with hardware_id == "localizer.amcl" (shared with
+ * AmclConvergenceEvaluator, in the same package). Takes control of "cmd_vel"
+ * (requires_control() == true) and rotates slowly in place each RT cycle, re-checking the same
+ * covariance trace the evaluator reads, until it drops back under threshold — or until `timeout`
+ * elapses without that happening, at which point it gives up (stops, returns FAILED) rather than
+ * spinning forever.
+ */
+class AmclRelocalizeMitigation : public easynav::RecoveryMitigationBase
+{
+public:
+  AmclRelocalizeMitigation() = default;
+  ~AmclRelocalizeMitigation() = default;
+
+  void on_initialize() override;
+
+  bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override;
+  bool requires_control() const override {return true;}
+
+protected:
+  void on_start(NavState & nav_state) override;
+  RecoveryStatus on_cycle(NavState & nav_state) override;
+
+private:
+  /// @brief Angular speed commanded while rotating in place (rad/s, "slowly").
+  double rotation_speed_ {0.3};
+
+  /// @brief Seconds to keep rotating before giving up. See the class doc comment.
+  double timeout_ {5.0};
+
+  /// @brief Same threshold as AmclConvergenceEvaluator by default — a deliberate simplification
+  /// (no hysteresis between evaluator and mitigator thresholds yet), same as already noted for
+  /// ObstacleTooCloseEvaluator/SafeRetreatRecovery.
+  double covariance_threshold_ {1.0};
+
+  rclcpp::Time start_time_;
+};
+
+}  // namespace easynav
+
+#endif  // EASYNAV_COSTMAP_LOCALIZER__AMCLRELOCALIZEMITIGATION_HPP_

localizers/easynav_costmap_localizer/package.xml

@@ -21,6 +21,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
 
   <test_depend>rclcpp_lifecycle</test_depend>

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp

@@ -307,12 +307,25 @@ void printTransform(const tf2::Transform & tf)
             << rot.w() << "]\n";
 }
 
+namespace
+{
+// Position + yaw dispersion in one scalar, from the 6x6 row-major covariance that get_pose()
+// already fills (indices 0/7 = var_x/var_y, 35 = var_yaw). Consumed by AmclConvergenceEvaluator
+// under the fixed key below.
+double covariance_trace(const nav_msgs::msg::Odometry & odom)
+{
+  return odom.pose.covariance[0] + odom.pose.covariance[7] + odom.pose.covariance[35];
+}
+}  // namespace
+
 void
 AMCLLocalizer::update_rt(NavState & nav_state)
 {
   predict(nav_state);
 
-  nav_state.set("robot_pose", get_pose());
+  const auto odom = get_pose();
+  nav_state.set("robot_pose", odom);
+  nav_state.set("localizer.amcl.covariance_trace", covariance_trace(odom));
 }
 
 void
@@ -325,7 +338,9 @@ AMCLLocalizer::update(NavState & nav_state)
     last_reseed_ = get_node()->now();
   }
 
-  nav_state.set("robot_pose", get_pose());
+  const auto odom = get_pose();
+  nav_state.set("robot_pose", odom);
+  nav_state.set("localizer.amcl.covariance_trace", covariance_trace(odom));
 
   publishParticles();
 }