AmclRelocalizeMitigation: pose resets instead of rotation; AMCL refines after a reset

localizers/easynav_costmap_localizer/CMakeLists.txt

@@ -72,8 +72,10 @@ target_link_libraries(amcl_relocalize_mitigation PUBLIC
   easynav_core::easynav_core
   easynav_diagnostic_recovery::easynav_diagnostic_recovery
   pluginlib::pluginlib
+  rclcpp::rclcpp
   ${diagnostic_msgs_TARGETS}
   ${geometry_msgs_TARGETS}
+  ${nav_msgs_TARGETS}
 )
 
 install(

localizers/easynav_costmap_localizer/README.md

@@ -45,6 +45,8 @@ All parameters are declared under the plugin namespace, i.e., `/<node_fqn>/easyn
 | `<plugin>.noise_translation_to_rotation` | `double` | `0.01` | Translation-to-rotation noise coupling. |
 | `<plugin>.min_noise_xy` | `double` | `0.05` | XY noise (m) of each particle reseeded around its parent, and minimum XY spread when the initial pose is set. |
 | `<plugin>.min_noise_yaw` | `double` | `0.05` | Yaw noise (rad) of each particle reseeded around its parent, and minimum yaw spread when the initial pose is set. |
+| `<plugin>.reset_refine_time` | `double` | `3.0` | After an initial pose (e.g. RViz2's, or `AmclRelocalizeMitigation`'s), for this long (s), the reseeds spread the new particles more, so that the cloud keeps refining the pose with the robot standing still. `0` disables it. |
+| `<plugin>.reset_refine_noise` | `double` | `0.25` | That extra spread at the start, as a fraction of the initial pose's deviation; it falls to `min_noise_*` by the end of `reset_refine_time`. `0` disables it. |
 | `<plugin>.min_height` | `double` | `0.1` | Points lower than this (m, `base_footprint` frame) are the floor and do not correct the particles. Lower it for sensors mounted lower (e.g. a laser 0.095 m high). |
 | `<plugin>.compute_odom_from_tf` | `bool` | `false` | If true, read odometry from TF (odom->base_footprint) instead of /odom topic. |
 

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp

@@ -236,6 +236,20 @@ class AMCLLocalizer : public LocalizerMethodBase
   /// Points below this height (m, robot footprint frame) are ignored: floor hits
   double min_height_ {0.1};
 
+  /// After an initial pose, for this long (s), the reseeds spread the new particles more: a robot
+  /// standing still has no motion noise, and the cloud would settle where it first converged.
+  double reset_refine_time_ {3.0};
+
+  /// That extra spread at the start, as a fraction of the initial pose's deviation; it falls to
+  /// min_noise_* by the end of reset_refine_time.
+  double reset_refine_noise_ {0.25};
+
+  /// Refining after an initial pose: since when, and its starting deviations (m, rad).
+  bool refining_ {false};
+  rclcpp::Time refine_start_;
+  double refine_xy_ {0.0};
+  double refine_yaw_ {0.0};
+
   /// Whether to use TFs to compute odom
   bool compute_odom_from_tf_ {false};
 

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp

@@ -18,6 +18,10 @@
 #ifndef EASYNAV_COSTMAP_LOCALIZER__AMCLRELOCALIZEMITIGATION_HPP_
 #define EASYNAV_COSTMAP_LOCALIZER__AMCLRELOCALIZEMITIGATION_HPP_
 
+#include <random>
+
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "rclcpp/publisher.hpp"
 #include "rclcpp/time.hpp"
 
 #include "easynav_diagnostic_recovery/RecoveryMitigationBase.hpp"
@@ -27,15 +31,23 @@ namespace easynav
 
 /**
  * @class AmclRelocalizeMitigation
- * @brief Level-1 movement mitigation: rotates in place to help AMCL relocalize.
+ * @brief Level-1 mitigation: resets AMCL's pose around where the robot was when it got lost.
  *
  * 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 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.
+ * AmclConvergenceEvaluator, in the same package). Holds the robot still (requires_control())
+ * and publishes an initial pose on "initialpose", with RViz2's "2D Pose Estimate" covariance, up
+ * to `attempts` times:
+ *  - the first one at the robot's pose when the recovery started;
+ *  - the next ones at that pose moved up to `max_offset` m in a random direction.
+ *
+ * Each attempt has `attempt_timeout` s to converge, by the evaluator's criteria: the covariance
+ * trace under `covariance_threshold`, and the match score and quality ratio (if reported) at
+ * least `match_threshold` and `quality_ratio_threshold`. Only checked after the localizer has had
+ * time to correct the new pose (the scores before it are of the old one). SUCCEEDED when an
+ * attempt converges, FAILED when none does, or at once if there is no robot pose to reset to.
+ *
+ * If AMCL has already converged when it starts (the diagnostic that selected it was evaluated
+ * before), it resets nothing: SUCCEEDED.
  */
 class AmclRelocalizeMitigation : public easynav_diagnostic_recovery::RecoveryMitigationBase
 {
@@ -53,11 +65,20 @@ class AmclRelocalizeMitigation : public easynav_diagnostic_recovery::RecoveryMit
   easynav_diagnostic_recovery::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 Publishes the pose of attempt `attempt_` and restarts its clock.
+  void start_attempt(NavState & nav_state);
+
+  /// @brief Whether AMCL has converged (see the class doc comment).
+  bool converged(const NavState & nav_state) const;
 
-  /// @brief Seconds to keep rotating before giving up. See the class doc comment.
-  double timeout_ {5.0};
+  /// @brief Initial poses tried, the first one without offset.
+  int attempts_ {5};
+
+  /// @brief Seconds each attempt has to converge.
+  double attempt_timeout_ {5.0};
+
+  /// @brief Largest offset (m) of the attempts after the first one.
+  double max_offset_ {1.0};
 
   /// @brief Same threshold as AmclConvergenceEvaluator by default — a deliberate simplification
   /// (no hysteresis between evaluator and mitigator thresholds yet), same as already noted for
@@ -68,7 +89,18 @@ class AmclRelocalizeMitigation : public easynav_diagnostic_recovery::RecoveryMit
   /// checked). Same as AmclConvergenceEvaluator's by default.
   double match_threshold_ {0.5};
 
-  rclcpp::Time start_time_;
+  /// @brief And the quality ratio at least this (0: not checked). Same as
+  /// AmclConvergenceEvaluator's by default.
+  double quality_ratio_threshold_ {0.5};
+
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
+  std::mt19937 rng_ {std::random_device{}()};
+
+  bool has_lost_pose_ {false};
+  bool converged_at_start_ {false};  ///< Nothing to do: SUCCEEDED at the first cycle.
+  geometry_msgs::msg::PoseWithCovarianceStamped lost_pose_;  ///< Where the robot got lost.
+  int attempt_ {0};
+  rclcpp::Time attempt_start_;
 };
 
 }  // namespace easynav

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp

@@ -250,6 +250,12 @@ 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_);
+  easynav::declare_parameter_if_absent<double>(
+    *node, plugin_name + ".reset_refine_time", reset_refine_time_);
+  easynav::declare_parameter_if_absent<double>(
+    *node, plugin_name + ".reset_refine_noise", reset_refine_noise_);
+  node->get_parameter<double>(plugin_name + ".reset_refine_time", reset_refine_time_);
+  node->get_parameter<double>(plugin_name + ".reset_refine_noise", reset_refine_noise_);
   double alpha_slow = 0.001, alpha_fast = 0.1;
   easynav::declare_parameter_if_absent<double>(
     *node, plugin_name + ".recovery_alpha_slow", alpha_slow);
@@ -284,6 +290,9 @@ AMCLLocalizer::on_initialize()
   {
     throw std::runtime_error("AMCLLocalizer: standard deviations must be positive");
   }
+  if (!(reset_refine_time_ >= 0.0) || !(reset_refine_noise_ >= 0.0)) {
+    throw std::runtime_error("AMCLLocalizer: reset_refine_time and reset_refine_noise >= 0");
+  }
 
   RCLCPP_INFO(node->get_logger(), "Initialized AMCL pose with %d particles", num_particles);
   RCLCPP_INFO(
@@ -569,6 +578,11 @@ AMCLLocalizer::init_pose_callback(
 
   // Prevent immediate reseed after initialization
   last_reseed_ = get_node()->now();
+  // Then the reseeds refine it (see reset_refine_time)
+  refining_ = reset_refine_time_ > 0.0 && reset_refine_noise_ > 0.0;
+  refine_start_ = last_reseed_;
+  refine_xy_ = reset_refine_noise_ * std::sqrt(0.5 * (l00 * l00 + l10 * l10 + l11 * l11));
+  refine_yaw_ = reset_refine_noise_ * yaw_stddev;
   // A new pose: the scores before it are no reference any more.
   quality_.reset();
 
@@ -796,9 +810,21 @@ AMCLLocalizer::reseed(const Costmap2D * map, std::size_t n_random)
     });
 
   // Fixed noise around the parent: noise from the spread of the best ones made the cloud grow
-  // without bound at every reseed when the weights do not tell the particles apart.
-  std::normal_distribution<double> xy_noise(0.0, min_noise_xy_);
-  std::normal_distribution<double> yaw_noise(0.0, min_noise_yaw_);
+  // without bound at every reseed when the weights do not tell the particles apart. Only after
+  // an initial pose, more for a while, falling to it: the particles can still move to the pose
+  // that matches best without the robot moving.
+  double sd_xy = min_noise_xy_, sd_yaw = min_noise_yaw_;
+  if (refining_) {
+    const double left = 1.0 - (get_node()->now() - refine_start_).seconds() / reset_refine_time_;
+    if (left > 0.0) {
+      sd_xy = std::max(sd_xy, left * refine_xy_);
+      sd_yaw = std::max(sd_yaw, left * refine_yaw_);
+    } else {
+      refining_ = false;
+    }
+  }
+  std::normal_distribution<double> xy_noise(0.0, sd_xy);
+  std::normal_distribution<double> yaw_noise(0.0, sd_yaw);
   std::normal_distribution<double> index_dist(0.0, std::max(0.05 * static_cast<double>(N), 1e-12));
 
   for (std::size_t i = N_top; i < N; ++i) {

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp

@@ -15,8 +15,13 @@
 /// \file
 /// \brief Implementation of the AmclRelocalizeMitigation class.
 
+#include <algorithm>
+#include <cmath>
+#include <string>
+
 #include "easynav_common/Parameters.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/create_publisher.hpp"
 
 #include "easynav_common/RTTFBuffer.hpp"
 
@@ -25,25 +30,61 @@
 namespace easynav
 {
 
+namespace
+{
+
+// RViz2's "2D Pose Estimate": 0.5 m in x and y, pi/12 rad in yaw (standard deviations)
+constexpr double kInitialVarianceXY = 0.25;
+constexpr double kInitialVarianceYaw = 0.06853891945200942;
+
+// The scores of the old pose may still be there: an attempt is checked after this (s)
+constexpr double kSettleTime = 1.0;
+
+}  // namespace
+
 void AmclRelocalizeMitigation::on_initialize()
 {
   auto node = get_node();
   const auto & plugin_name = get_plugin_name();
 
+  easynav::declare_parameter_if_absent<int>(*node, plugin_name + ".attempts", attempts_);
   easynav::declare_parameter_if_absent<double>(
-    *node, plugin_name + ".rotation_speed",
-    rotation_speed_);
-  easynav::declare_parameter_if_absent<double>(*node, plugin_name + ".timeout", timeout_);
+    *node, plugin_name + ".attempt_timeout", attempt_timeout_);
+  easynav::declare_parameter_if_absent<double>(*node, plugin_name + ".max_offset", max_offset_);
   easynav::declare_parameter_if_absent<double>(
-    *node, plugin_name + ".covariance_threshold",
-    covariance_threshold_);
-
-  node->get_parameter<double>(plugin_name + ".rotation_speed", rotation_speed_);
-  node->get_parameter<double>(plugin_name + ".timeout", timeout_);
-  node->get_parameter<double>(plugin_name + ".covariance_threshold", covariance_threshold_);
+    *node, 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_);
+
+  node->get_parameter<int>(plugin_name + ".attempts", attempts_);
+  node->get_parameter<double>(plugin_name + ".attempt_timeout", attempt_timeout_);
+  node->get_parameter<double>(plugin_name + ".max_offset", max_offset_);
+  node->get_parameter<double>(plugin_name + ".covariance_threshold", covariance_threshold_);
   node->get_parameter<double>(plugin_name + ".match_threshold", match_threshold_);
+  node->get_parameter<double>(
+    plugin_name + ".quality_ratio_threshold", quality_ratio_threshold_);
+
+  if (attempts_ < 1 || !(attempt_timeout_ > 0.0) || !(max_offset_ >= 0.0)) {
+    throw std::runtime_error(
+            "[" + plugin_name + "] invalid parameters: attempts >= 1, attempt_timeout > 0, "
+            "max_offset >= 0");
+  }
+
+  // It no longer rotates: these are not used
+  const auto & overrides = node->get_node_parameters_interface()->get_parameter_overrides();
+  for (const char * removed : {"rotation_speed", "timeout"}) {
+    if (overrides.count(plugin_name + "." + removed) > 0) {
+      RCLCPP_WARN(
+        node->get_logger(), "[%s] '%s' has no effect any more: remove it (the pose is reset "
+        "instead: attempts, attempt_timeout, max_offset)", plugin_name.c_str(), removed);
+    }
+  }
+
+  // A plain publisher, as RViz2's: active whatever the node's lifecycle state
+  initial_pose_pub_ = rclcpp::create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    *node, "initialpose", rclcpp::QoS(10));
 }
 
 bool AmclRelocalizeMitigation::can_handle(
@@ -55,48 +96,96 @@ bool AmclRelocalizeMitigation::can_handle(
 
 void AmclRelocalizeMitigation::on_start(NavState & nav_state)
 {
-  start_time_ = get_node()->now();
+  attempt_ = 0;
+  // Selected on a diagnostic evaluated before AMCL converged: resetting it would lose it again
+  converged_at_start_ = converged(nav_state);
+  if (converged_at_start_) {
+    return;
+  }
+  has_lost_pose_ = nav_state.has("robot_pose");
+  if (!has_lost_pose_) {
+    report(
+      nav_state, rcl_interfaces::msg::Log::ERROR,
+      "AmclRelocalizeMitigation [" + get_plugin_name() + "]: no robot pose to reset to");
+    return;
+  }
+  lost_pose_ = geometry_msgs::msg::PoseWithCovarianceStamped();
+  lost_pose_.pose.pose = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
+  start_attempt(nav_state);
+}
+
+void AmclRelocalizeMitigation::start_attempt(NavState & nav_state)
+{
+  auto pose = lost_pose_;
+  pose.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().map_frame;
+  pose.header.stamp = get_node()->now();
+  if (attempt_ > 0) {
+    // Uniform over the disc of radius max_offset
+    std::uniform_real_distribution<double> unit(0.0, 1.0);
+    const double r = max_offset_ * std::sqrt(unit(rng_));
+    const double a = 2.0 * M_PI * unit(rng_);
+    pose.pose.pose.position.x += r * std::cos(a);
+    pose.pose.pose.position.y += r * std::sin(a);
+  }
+  pose.pose.covariance[0] = kInitialVarianceXY;
+  pose.pose.covariance[7] = kInitialVarianceXY;
+  pose.pose.covariance[35] = kInitialVarianceYaw;
+  initial_pose_pub_->publish(pose);
+  attempt_start_ = get_node()->now();
+
+  const auto & p = pose.pose.pose.position;
   report(
     nav_state, rcl_interfaces::msg::Log::WARN,
-    "AmclRelocalizeMitigation [" + get_plugin_name() +
-    "]: localization diverged, rotating in place to relocalize");
+    "AmclRelocalizeMitigation [" + get_plugin_name() + "]: localization lost, resetting the " +
+    "pose to (" + std::to_string(p.x) + ", " + std::to_string(p.y) + "), attempt " +
+    std::to_string(attempt_ + 1) + " of " + std::to_string(attempts_));
 }
 
-easynav_diagnostic_recovery::RecoveryStatus AmclRelocalizeMitigation::on_cycle(NavState & nav_state)
+bool AmclRelocalizeMitigation::converged(const NavState & nav_state) const
 {
   // Written by AMCLLocalizer from both its RT and non-RT cycles; read here from
   // RecoveryManagerNode's RT cycle (this mitigation requires_control()), so get_safe() (a
   // snapshot copy) is required, not get(). See NavState's own get()/get_safe() guidance.
-  if (nav_state.has("localizer.amcl.covariance_trace")) {
-    const double trace = nav_state.get_safe<double>("localizer.amcl.covariance_trace");
-    // A compact cloud is not enough: it must also match the map.
-    const bool matches = match_threshold_ <= 0.0 ||
-      !nav_state.has("localizer.amcl.match_score") ||
-      nav_state.get_safe<double>("localizer.amcl.match_score") >= match_threshold_;
-    if (trace <= covariance_threshold_ && matches) {
-      stop_robot(nav_state);
-      return easynav_diagnostic_recovery::RecoveryStatus::SUCCEEDED;
-    }
+  if (!nav_state.has("localizer.amcl.covariance_trace")) {
+    return false;
   }
+  const double trace = nav_state.get_safe<double>("localizer.amcl.covariance_trace");
+  // A compact cloud is not enough: it must also match the map.
+  const bool matches = match_threshold_ <= 0.0 ||
+    !nav_state.has("localizer.amcl.match_score") ||
+    nav_state.get_safe<double>("localizer.amcl.match_score") >= match_threshold_;
+  const bool steady = quality_ratio_threshold_ <= 0.0 ||
+    !nav_state.has("localizer.amcl.quality_ratio") ||
+    nav_state.get_safe<double>("localizer.amcl.quality_ratio") >= quality_ratio_threshold_;
+  return trace <= covariance_threshold_ && matches && steady;
+}
 
-  if ((get_node()->now() - start_time_).seconds() >= timeout_) {
-    report(
-      nav_state, rcl_interfaces::msg::Log::ERROR,
-      "AmclRelocalizeMitigation [" + get_plugin_name() + "]: gave up after " +
-      std::to_string(timeout_) + " s without relocalizing");
-    stop_robot(nav_state);
+easynav_diagnostic_recovery::RecoveryStatus AmclRelocalizeMitigation::on_cycle(NavState & nav_state)
+{
+  stop_robot(nav_state);  // still while AMCL converges
+  if (converged_at_start_) {
+    return easynav_diagnostic_recovery::RecoveryStatus::SUCCEEDED;
+  }
+  if (!has_lost_pose_) {
     return easynav_diagnostic_recovery::RecoveryStatus::FAILED;
   }
 
-  geometry_msgs::msg::TwistStamped cmd;
-  if (auto node = get_node()) {
-    cmd.header.stamp = node->now();
+  const double elapsed = (get_node()->now() - attempt_start_).seconds();
+  if (elapsed >= std::min(kSettleTime, attempt_timeout_) && converged(nav_state)) {
+    return easynav_diagnostic_recovery::RecoveryStatus::SUCCEEDED;
   }
-  cmd.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().robot_frame;
-  cmd.twist.angular.z = rotation_speed_;
-
-  command_velocity(nav_state, cmd);
-  return easynav_diagnostic_recovery::RecoveryStatus::RUNNING;
+  if (elapsed < attempt_timeout_) {
+    return easynav_diagnostic_recovery::RecoveryStatus::RUNNING;
+  }
+  if (++attempt_ < attempts_) {
+    start_attempt(nav_state);
+    return easynav_diagnostic_recovery::RecoveryStatus::RUNNING;
+  }
+  report(
+    nav_state, rcl_interfaces::msg::Log::ERROR,
+    "AmclRelocalizeMitigation [" + get_plugin_name() + "]: gave up after " +
+    std::to_string(attempts_) + " pose resets without relocalizing");
+  return easynav_diagnostic_recovery::RecoveryStatus::FAILED;
 }
 
 }  // namespace easynav