Costmap AMCL: detect a lost filter and replace the worst particles

localizers/easynav_costmap_localizer/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(Eigen3 REQUIRED NO_MODULE)
 
 add_library(${PROJECT_NAME} SHARED
   src/easynav_costmap_localizer/AMCLLocalizer.cpp
+  src/easynav_costmap_localizer/LocalizationQuality.cpp
 )
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp

@@ -31,6 +31,8 @@
 #include "tf2/LinearMath/Transform.hpp"
 #include "tf2_ros/transform_broadcaster.hpp"
 #include "easynav_core/LocalizerMethodBase.hpp"
+#include "easynav_costmap_common/costmap_2d.hpp"
+#include "easynav_costmap_localizer/LocalizationQuality.hpp"
 
 namespace easynav
 {
@@ -140,9 +142,21 @@ class AMCLLocalizer : public LocalizerMethodBase
   void correct(NavState & nav_state);
 
   /**
-   * @brief Re-initializes the particle cloud if necessary.
+   * @brief Replaces the worse half of the particles with copies of the better half.
+   * @param map With it, the `n_random` worst particles get random free poses of the map instead
+   *   (kidnapping recovery).
+   * @param n_random How many of the worst particles to place at random.
    */
-  void reseed();
+  void reseed(const Costmap2D * map = nullptr, std::size_t n_random = 0);
+
+  /// @brief How many particles to place at random at the next reseed (from the quality drop).
+  std::size_t random_particles_to_inject() const;
+
+  /// @brief How well the particles explain the sensors (see LocalizationQuality).
+  LocalizationQuality quality_;
+
+  /// @brief At most this share of the particles is placed at random in a reseed.
+  double random_particles_max_fraction_ {0.25};
 
   /// TF broadcaster to publish map to base_footprint transform.
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp

@@ -18,7 +18,10 @@
 #ifndef EASYNAV_COSTMAP_LOCALIZER__AMCLCONVERGENCEEVALUATOR_HPP_
 #define EASYNAV_COSTMAP_LOCALIZER__AMCLCONVERGENCEEVALUATOR_HPP_
 
+#include <optional>
+
 #include "easynav_diagnostic_recovery/RecoveryEvaluatorBase.hpp"
+#include "rclcpp/time.hpp"
 
 namespace easynav
 {
@@ -32,8 +35,14 @@ namespace easynav
  * 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
+ * Three signs, any of which is an ERROR:
+ *  - "localizer.amcl.covariance_trace" above `covariance_threshold`: the particles spread out;
+ *  - "localizer.amcl.match_score" below `match_threshold` for `debounce_duration`: the scans do
+ *    not match the map where the filter places the robot (a compact cloud can still be wrong);
+ *  - "localizer.amcl.quality_ratio" below `quality_ratio_threshold` for `debounce_duration`:
+ *    the match has just dropped well below its usual level (e.g. the robot was kidnapped).
+ *
+ * All written by AMCLLocalizer. 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.
  */
@@ -53,6 +62,20 @@ class AmclConvergenceEvaluator : public easynav_diagnostic_recovery::RecoveryEva
   /// diverged. Starting point only — depends on sensor/robot and needs tuning on the real
   /// platform.
   double covariance_threshold_ {1.0};
+
+  /// @brief Best particle's score (fraction of scan points on obstacles of the map) below which
+  /// the filter is lost. 0 disables it. Depends on the map, sensor and resolution: in the Kobuki
+  /// playground, 0.75 localized and 0.25-0.35 lost (also converged at a wrong pose).
+  double match_threshold_ {0.5};
+
+  /// @brief Fast/slow average ratio of the scores below which the match has dropped.
+  double quality_ratio_threshold_ {0.5};
+
+  /// @brief Seconds the match or the ratio must stay low before it is an ERROR.
+  double debounce_duration_ {2.0};
+
+  /// @brief Since when the match or the ratio is low, if it is.
+  std::optional<rclcpp::Time> low_since_;
 };
 
 }  // namespace easynav

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp

@@ -32,7 +32,8 @@ namespace easynav
  * 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`
+ * covariance trace the evaluator reads, until it drops back under threshold and the match score
+ * (if checked) is back above `match_threshold` — or until `timeout`
  * elapses without that happening, at which point it gives up (stops, returns FAILED) rather than
  * spinning forever.
  */
@@ -63,6 +64,10 @@ class AmclRelocalizeMitigation : public easynav_diagnostic_recovery::RecoveryMit
   /// ObstacleTooCloseEvaluator/SafeRetreatRecovery.
   double covariance_threshold_ {1.0};
 
+  /// @brief And, if the localizer reports it, the match score must be at least this (0: not
+  /// checked). Same as AmclConvergenceEvaluator's by default.
+  double match_threshold_ {0.5};
+
   rclcpp::Time start_time_;
 };
 

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/LocalizationQuality.hpp

@@ -0,0 +1,79 @@
+// 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");
+// 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_COSTMAP_LOCALIZER__LOCALIZATIONQUALITY_HPP_
+#define EASYNAV_COSTMAP_LOCALIZER__LOCALIZATIONQUALITY_HPP_
+
+#include <cstddef>
+
+namespace easynav
+{
+
+/// @brief How well the particles explain what the sensors see, in absolute terms.
+/**
+ * Each correction gives two scores in [0, 1]: the best particle's and the mean of all
+ * particles' (the fraction of points that land on obstacles of the map). The covariance only
+ * tells how spread the particles are; a lost filter can be compact and wrong, and these scores
+ * drop when it is.
+ *
+ *  - match_score(): the best particle's score, smoothed (fast average).
+ *  - quality_ratio(): fast / slow average of the mean score (Augmented MCL, Thrun et al.,
+ *    Probabilistic Robotics, 8.3.5). Near 1 while the scores keep their usual level; well
+ *    below 1 right after they drop, e.g. when the robot is kidnapped.
+ *  - injection_probability(): max(0, 1 - fast / slow), the share of particles to replace with
+ *    random ones.
+ *
+ * Each average starts as the plain mean of the scores so far (until its alpha is smaller), so
+ * the first, still unconverged corrections do not set the level for long.
+ */
+class LocalizationQuality
+{
+public:
+  /// @param alpha_slow, alpha_fast Smoothing per correction, 0 < alpha_slow < alpha_fast <= 1.
+  ///   Either 0 disables the slow/fast averages (ratio 1, no injection).
+  explicit LocalizationQuality(double alpha_slow = 0.001, double alpha_fast = 0.1);
+
+  /// @brief Takes the scores of a correction.
+  /// @param mean_score Mean score of all particles.
+  /// @param best_score Best particle's score.
+  void add(double mean_score, double best_score);
+
+  /// @brief Whether any correction was taken.
+  bool has_data() const {return has_data_;}
+  double match_score() const {return best_;}
+  double slow() const {return slow_;}
+  double fast() const {return fast_;}
+  double quality_ratio() const;
+  double injection_probability() const;
+
+  /// @brief Forgets the scores (e.g. after a new initial pose).
+  void reset();
+
+private:
+  bool averaging() const {return alpha_slow_ > 0.0 && alpha_fast_ > 0.0;}
+
+  double alpha_slow_;
+  double alpha_fast_;
+  bool has_data_ {false};
+  /// Corrections taken: the first ones use their plain mean (warm-up).
+  std::size_t count_ {0};
+  double best_ {0.0};
+  double slow_ {0.0};
+  double fast_ {0.0};
+};
+
+}  // namespace easynav
+
+#endif  // EASYNAV_COSTMAP_LOCALIZER__LOCALIZATIONQUALITY_HPP_

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp

@@ -247,6 +247,19 @@ AMCLLocalizer::on_initialize()
   node->get_parameter<double>(plugin_name + ".min_noise_yaw", min_noise_yaw_);
   easynav::declare_parameter_if_absent<double>(*node, plugin_name + ".min_height", min_height_);
   node->get_parameter<double>(plugin_name + ".min_height", min_height_);
+  double alpha_slow = 0.001, alpha_fast = 0.1;
+  easynav::declare_parameter_if_absent<double>(
+    *node, plugin_name + ".recovery_alpha_slow", alpha_slow);
+  easynav::declare_parameter_if_absent<double>(
+    *node, plugin_name + ".recovery_alpha_fast", alpha_fast);
+  easynav::declare_parameter_if_absent<double>(
+    *node, plugin_name + ".random_particles_max_fraction", random_particles_max_fraction_);
+  node->get_parameter<double>(plugin_name + ".recovery_alpha_slow", alpha_slow);
+  node->get_parameter<double>(plugin_name + ".recovery_alpha_fast", alpha_fast);
+  node->get_parameter<double>(
+    plugin_name + ".random_particles_max_fraction", random_particles_max_fraction_);
+  random_particles_max_fraction_ = std::clamp(random_particles_max_fraction_, 0.0, 0.5);
+  quality_ = LocalizationQuality(alpha_slow, alpha_fast);
   node->get_parameter<bool>(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_);
 
   node->get_parameter<double>(plugin_name + ".reseed_freq", reseed_freq);
@@ -358,13 +371,20 @@ AMCLLocalizer::update(NavState & nav_state)
   correct(nav_state);
 
   if ((get_node()->now() - last_reseed_).seconds() > reseed_time_) {
-    reseed();
+    const Costmap2D * map =
+      nav_state.has("map.base") ? &nav_state.get<Costmap2D>("map.base") : nullptr;
+    reseed(map, map ? random_particles_to_inject() : 0);
     last_reseed_ = get_node()->now();
   }
 
   const auto odom = get_pose();
   nav_state.set("robot_pose", odom);
   nav_state.set("localizer.amcl.covariance_trace", covariance_trace(odom));
+  if (quality_.has_data()) {
+    // Read by AmclConvergenceEvaluator and AmclRelocalizeMitigation.
+    nav_state.set("localizer.amcl.match_score", quality_.match_score());
+    nav_state.set("localizer.amcl.quality_ratio", quality_.quality_ratio());
+  }
 
   publishParticles();
 }
@@ -530,6 +550,8 @@ AMCLLocalizer::init_pose_callback(
 
   // Prevent immediate reseed after initialization
   last_reseed_ = get_node()->now();
+  // A new pose: the scores before it are no reference any more.
+  quality_.reset();
 
   // Compute map->basefootprint estimate
   tf2::Transform map2bf = getEstimatedPose();
@@ -666,6 +688,8 @@ AMCLLocalizer::correct(NavState & nav_state)
     return;
   }
 
+  double score_sum = 0.0, best_score = 0.0;
+  std::size_t scored = 0;
   for (auto & particle : particles_) {
     int hits = 0;
     int possible_hits = 0;
@@ -687,6 +711,16 @@ AMCLLocalizer::correct(NavState & nav_state)
 
     particle.hits += hits;
     particle.possible_hits += possible_hits;
+    if (possible_hits > 0) {
+      const double score = static_cast<double>(hits) / static_cast<double>(possible_hits);
+      score_sum += score;
+      best_score = std::max(best_score, score);
+      ++scored;
+    }
+  }
+  // Absolute scores, before the weights are normalized: they tell a lost filter apart.
+  if (scored > 0) {
+    quality_.add(score_sum / static_cast<double>(scored), best_score);
   }
 
   for (auto & particle : particles_) {
@@ -710,8 +744,16 @@ AMCLLocalizer::correct(NavState & nav_state)
   }
 }
 
+std::size_t
+AMCLLocalizer::random_particles_to_inject() const
+{
+  const double n = static_cast<double>(particles_.size());
+  const double share = std::min(quality_.injection_probability(), random_particles_max_fraction_);
+  return static_cast<std::size_t>(std::floor(share * n + 0.5));
+}
+
 void
-AMCLLocalizer::reseed()
+AMCLLocalizer::reseed(const Costmap2D * map, std::size_t n_random)
 {
   if (particles_.empty()) {return;}
   if (particles_[0].possible_hits == 0) {return;}
@@ -755,6 +797,31 @@ AMCLLocalizer::reseed()
     particles_[i].weight = ref_particle.weight;
   }
 
+  // The worst ones (the last after sorting) go to random free poses of the map: if the filter is
+  // lost, one of them may land near the true pose and win the next reseeds.
+  if (map && n_random > 0 && map->getSizeInCellsX() > 0 && map->getSizeInCellsY() > 0) {
+    n_random = std::min(n_random, N - N_top);
+    std::uniform_int_distribution<unsigned int> cell_x(0, map->getSizeInCellsX() - 1);
+    std::uniform_int_distribution<unsigned int> cell_y(0, map->getSizeInCellsY() - 1);
+    std::uniform_real_distribution<double> any_yaw(-M_PI, M_PI);
+    const double min_weight = particles_[N_top - 1].weight;
+    std::size_t placed = 0;
+    for (std::size_t tries = 0; placed < n_random && tries < 100 * n_random; ++tries) {
+      const unsigned int cx = cell_x(rng_), cy = cell_y(rng_);
+      const uint8_t cost = map->getCost(cx, cy);
+      if (cost >= easynav::INSCRIBED_INFLATED_OBSTACLE) {continue;}  // obstacle or unknown
+      double wx, wy;
+      map->mapToWorld(cx, cy, wx, wy);
+      tf2::Quaternion q;
+      q.setRPY(0.0, 0.0, any_yaw(rng_));
+      auto & p = particles_[N - 1 - placed];
+      p.pose.setOrigin(tf2::Vector3(wx, wy, 0.0));
+      p.pose.setRotation(q);
+      p.weight = min_weight;
+      ++placed;
+    }
+  }
+
   for (auto & particle : particles_) {
     particle.hits = 0;
     particle.possible_hits = 0;

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp

@@ -30,6 +30,15 @@ void AmclConvergenceEvaluator::on_initialize()
     *node, plugin_name + ".covariance_threshold",
     covariance_threshold_);
   node->get_parameter<double>(plugin_name + ".covariance_threshold", covariance_threshold_);
+  easynav::declare_parameter_if_absent<double>(
+    *node, plugin_name + ".match_threshold", match_threshold_);
+  easynav::declare_parameter_if_absent<double>(
+    *node, plugin_name + ".quality_ratio_threshold", quality_ratio_threshold_);
+  easynav::declare_parameter_if_absent<double>(
+    *node, plugin_name + ".debounce_duration", debounce_duration_);
+  node->get_parameter<double>(plugin_name + ".match_threshold", match_threshold_);
+  node->get_parameter<double>(plugin_name + ".quality_ratio_threshold", quality_ratio_threshold_);
+  node->get_parameter<double>(plugin_name + ".debounce_duration", debounce_duration_);
 }
 
 void AmclConvergenceEvaluator::update(NavState & nav_state)
@@ -46,19 +55,49 @@ void AmclConvergenceEvaluator::update(NavState & nav_state)
     return;
   }
 
+  auto add_value = [&status](const std::string & key, double value) {
+      diagnostic_msgs::msg::KeyValue kv;
+      kv.key = key;
+      kv.value = std::to_string(value);
+      status.values.push_back(kv);
+    };
+
   // Written by AMCLLocalizer from both its RT and non-RT cycles; this evaluator runs on
   // RecoveryManagerNode's non-RT cycle, so get_safe() (a snapshot copy) is required here, not
   // get(). See NavState's own get()/get_safe() guidance.
   const double trace = nav_state.get_safe<double>("localizer.amcl.covariance_trace");
+  add_value("covariance_trace", trace);
+
+  // The scores, if the localizer reports them.
+  std::string low_reason;
+  if (nav_state.has("localizer.amcl.match_score")) {
+    const double match = nav_state.get_safe<double>("localizer.amcl.match_score");
+    add_value("match_score", match);
+    if (match_threshold_ > 0.0 && match < match_threshold_) {
+      low_reason = "scans do not match the map (lost)";
+    }
+  }
+  if (nav_state.has("localizer.amcl.quality_ratio")) {
+    const double ratio = nav_state.get_safe<double>("localizer.amcl.quality_ratio");
+    add_value("quality_ratio", ratio);
+    if (low_reason.empty() && ratio < quality_ratio_threshold_) {
+      low_reason = "scan match dropped (kidnapped?)";
+    }
+  }
+
+  const auto now = get_node()->now();
+  if (low_reason.empty()) {
+    low_since_.reset();
+  } else if (!low_since_) {
+    low_since_ = now;
+  }
 
   if (trace > covariance_threshold_) {
     status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
     status.message = "particle filter diverged";
-
-    diagnostic_msgs::msg::KeyValue trace_kv;
-    trace_kv.key = "covariance_trace";
-    trace_kv.value = std::to_string(trace);
-    status.values.push_back(trace_kv);
+  } else if (low_since_ && (now - *low_since_).seconds() >= debounce_duration_) {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    status.message = low_reason;
   }
 
   publish_diagnostic(nav_state, status);