From 37a7ff373e9cb3bb35649655694f5be5076318cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 3 Nov 2026 11:30:05 +0100 Subject: [PATCH] AmclRelocalizeMitigation: pose resets instead of rotation; AMCL refines after a reset MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Closes #173. Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_localizer/CMakeLists.txt | 2 + .../easynav_costmap_localizer/README.md | 2 + .../AMCLLocalizer.hpp | 14 + .../AmclRelocalizeMitigation.hpp | 56 +- .../AMCLLocalizer.cpp | 32 +- .../AmclRelocalizeMitigation.cpp | 165 ++++-- .../amcl_relocalize_mitigation_tests.cpp | 506 ++++++++++++++++-- .../tests/costmap_localizer_reseed_tests.cpp | 124 +++++ .../easynav_diagnostic_recovery/README.md | 2 +- 9 files changed, 793 insertions(+), 110 deletions(-) diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index c00357b4..6207f6db 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/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( diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index ded99e90..60e83c39 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -45,6 +45,8 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.noise_translation_to_rotation` | `double` | `0.01` | Translation-to-rotation noise coupling. | | `.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. | | `.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. | +| `.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. | +| `.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. | | `.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). | | `.compute_odom_from_tf` | `bool` | `false` | If true, read odometry from TF (odom->base_footprint) instead of /odom topic. | diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp index 7bb54a79..c195dd48 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/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}; diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp index 34ef0060..e64ecaf5 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp +++ b/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 + +#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::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 diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 92aec7d6..6e3ae690 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -250,6 +250,12 @@ AMCLLocalizer::on_initialize() node->get_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); easynav::declare_parameter_if_absent(*node, plugin_name + ".min_height", min_height_); node->get_parameter(plugin_name + ".min_height", min_height_); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".reset_refine_time", reset_refine_time_); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".reset_refine_noise", reset_refine_noise_); + node->get_parameter(plugin_name + ".reset_refine_time", reset_refine_time_); + node->get_parameter(plugin_name + ".reset_refine_noise", reset_refine_noise_); double alpha_slow = 0.001, alpha_fast = 0.1; easynav::declare_parameter_if_absent( *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 xy_noise(0.0, min_noise_xy_); - std::normal_distribution 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 xy_noise(0.0, sd_xy); + std::normal_distribution yaw_noise(0.0, sd_yaw); std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); for (std::size_t i = N_top; i < N; ++i) { diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp index d3700c24..564e067c 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp @@ -15,8 +15,13 @@ /// \file /// \brief Implementation of the AmclRelocalizeMitigation class. +#include +#include +#include + #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(*node, plugin_name + ".attempts", attempts_); easynav::declare_parameter_if_absent( - *node, plugin_name + ".rotation_speed", - rotation_speed_); - easynav::declare_parameter_if_absent(*node, plugin_name + ".timeout", timeout_); + *node, plugin_name + ".attempt_timeout", attempt_timeout_); + easynav::declare_parameter_if_absent(*node, plugin_name + ".max_offset", max_offset_); easynav::declare_parameter_if_absent( - *node, plugin_name + ".covariance_threshold", - covariance_threshold_); - - node->get_parameter(plugin_name + ".rotation_speed", rotation_speed_); - node->get_parameter(plugin_name + ".timeout", timeout_); - node->get_parameter(plugin_name + ".covariance_threshold", covariance_threshold_); + *node, plugin_name + ".covariance_threshold", covariance_threshold_); easynav::declare_parameter_if_absent( *node, plugin_name + ".match_threshold", match_threshold_); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".quality_ratio_threshold", quality_ratio_threshold_); + + node->get_parameter(plugin_name + ".attempts", attempts_); + node->get_parameter(plugin_name + ".attempt_timeout", attempt_timeout_); + node->get_parameter(plugin_name + ".max_offset", max_offset_); + node->get_parameter(plugin_name + ".covariance_threshold", covariance_threshold_); node->get_parameter(plugin_name + ".match_threshold", match_threshold_); + node->get_parameter( + 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( + *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("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 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("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("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("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("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("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 diff --git a/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp b/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp index d0a876bf..8ae073a7 100644 --- a/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp @@ -13,19 +13,101 @@ // limitations under the License. #include +#include +#include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_costmap_localizer/AmclRelocalizeMitigation.hpp" #include "easynav_core/VelocityCommand.hpp" +using easynav_diagnostic_recovery::RecoveryStatus; +using PoseMsg = geometry_msgs::msg::PoseWithCovarianceStamped; + +namespace +{ + +double yaw_of(const geometry_msgs::msg::Quaternion & q) +{ + return std::atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); +} + +void set_robot_pose(easynav::NavState & nav_state, double x, double y, double yaw) +{ + nav_msgs::msg::Odometry odom; + odom.header.frame_id = "map"; + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.orientation.z = std::sin(0.5 * yaw); + odom.pose.pose.orientation.w = std::cos(0.5 * yaw); + nav_state.set("robot_pose", odom); +} + +void set_localization(easynav::NavState & nav_state, double trace, double match = -1.0) +{ + nav_state.set("localizer.amcl.covariance_trace", trace); + if (match >= 0.0) { + nav_state.set("localizer.amcl.match_score", match); + } +} + +// The poses published on "initialpose", as AMCLLocalizer gets them +class PoseCatcher +{ +public: + PoseCatcher() + : node_(std::make_shared("initial_pose_catcher")) + { + sub_ = node_->create_subscription( + "initialpose", 10, [this](PoseMsg::SharedPtr msg) {poses.push_back(*msg);}); + executor_.add_node(node_); + } + + // Takes what has arrived + void spin() {executor_.spin_some();} + + // Waits for the publications so far: n poses in all + bool wait_for(std::size_t n, double timeout = 2.0) + { + const auto end = std::chrono::steady_clock::now() + + std::chrono::duration(timeout); + while (poses.size() < n && std::chrono::steady_clock::now() < end) { + spin(); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + spin(); + return poses.size() == n; + } + + // Waits until the publisher is matched, so that the first pose is not lost + void wait_matched(const std::shared_ptr & node) + { + const auto end = std::chrono::steady_clock::now() + std::chrono::seconds(2); + while (node->count_subscribers("initialpose") == 0 && std::chrono::steady_clock::now() < end) { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + } + + std::vector poses; + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::executors::SingleThreadedExecutor executor_; + rclcpp::Subscription::SharedPtr sub_; +}; + +} // namespace + class AmclRelocalizeMitigationTestCase : public ::testing::Test { protected: @@ -36,6 +118,7 @@ class AmclRelocalizeMitigationTestCase : public ::testing::Test } easynav::TFInfo tf_info; tf_info.robot_frame = "base_link"; + tf_info.map_frame = "map"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); } @@ -46,6 +129,40 @@ class AmclRelocalizeMitigationTestCase : public ::testing::Test mit->initialize(node, name); return mit; } + + static rclcpp::NodeOptions options( + const std::string & name, int attempts, double attempt_timeout, double max_offset = 1.0) + { + return rclcpp::NodeOptions() + .append_parameter_override(name + ".attempts", attempts) + .append_parameter_override(name + ".attempt_timeout", attempt_timeout) + .append_parameter_override(name + ".max_offset", max_offset); + } + + // Cycles until the status is not RUNNING (or the time is up), taking the poses published + static RecoveryStatus run( + const std::shared_ptr & mit, + easynav::NavState & nav_state, double timeout = 5.0, PoseCatcher * catcher = nullptr) + { + const auto end = std::chrono::steady_clock::now() + std::chrono::duration(timeout); + RecoveryStatus status = RecoveryStatus::RUNNING; + while (status == RecoveryStatus::RUNNING && std::chrono::steady_clock::now() < end) { + status = mit->internal_cycle(nav_state); + if (catcher) {catcher->spin();} + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + return status; + } + + static void expect_still(const easynav::NavState & nav_state) + { + const auto proposed = + easynav::velocity_command::peek(nav_state, easynav::VelocitySource::TAKEOVER); + ASSERT_TRUE(proposed.has_value()); + EXPECT_DOUBLE_EQ(proposed->twist.linear.x, 0.0); + EXPECT_DOUBLE_EQ(proposed->twist.linear.y, 0.0); + EXPECT_DOUBLE_EQ(proposed->twist.angular.z, 0.0); + } }; TEST_F(AmclRelocalizeMitigationTestCase, RequiresControl) @@ -74,108 +191,385 @@ TEST_F(AmclRelocalizeMitigationTestCase, CanHandleOnlyLocalizerAmclErrors) EXPECT_FALSE(mit->can_handle(not_an_error)); } -TEST_F(AmclRelocalizeMitigationTestCase, RotatesInPlaceWhileDivergedAndWithinTimeout) +TEST_F(AmclRelocalizeMitigationTestCase, DefaultsAreFiveAttemptsOfFiveSecondsWithinAMeter) +{ + auto node = std::make_shared("test_defaults_node"); + auto mit = make_mitigation(node, "amcl_mit_defaults"); + EXPECT_EQ(node->get_parameter("amcl_mit_defaults.attempts").as_int(), 5); + EXPECT_DOUBLE_EQ(node->get_parameter("amcl_mit_defaults.attempt_timeout").as_double(), 5.0); + EXPECT_DOUBLE_EQ(node->get_parameter("amcl_mit_defaults.max_offset").as_double(), 1.0); +} + +TEST_F(AmclRelocalizeMitigationTestCase, RejectsInvalidParameters) +{ + struct Case {const char * name; int attempts; double timeout; double offset;}; + for (const auto & c : {Case{"no_attempts", 0, 3.0, 1.0}, Case{"negative_attempts", -2, 3.0, 1.0}, + Case{"zero_timeout", 5, 0.0, 1.0}, Case{"negative_timeout", 5, -1.0, 1.0}, + Case{"negative_offset", 5, 3.0, -0.1}}) + { + const std::string name = std::string("amcl_mit_") + c.name; + auto node = std::make_shared( + std::string("test_invalid_") + c.name, options(name, c.attempts, c.timeout, c.offset)); + auto mit = std::make_shared(); + EXPECT_ANY_THROW(mit->initialize(node, name)) << c.name; + } +} + +TEST_F(AmclRelocalizeMitigationTestCase, FirstAttemptIsTheLostPoseWithRvizCovariance) { auto node = std::make_shared( - "test_rotate_node", - rclcpp::NodeOptions().append_parameter_override("amcl_mit2.rotation_speed", 0.4)); + "test_first_node", options("amcl_mit2", 5, 3.0)); auto mit = make_mitigation(node, "amcl_mit2"); + PoseCatcher catcher; + catcher.wait_matched(node); easynav::NavState nav_state; - nav_state.set("localizer.amcl.covariance_trace", 5.0); // well above default threshold + set_robot_pose(nav_state, 2.0, -1.5, 0.7); + set_localization(nav_state, 5.0, 0.1); // lost mit->internal_start(nav_state); - auto status = mit->internal_cycle(nav_state); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::RUNNING); + expect_still(nav_state); - EXPECT_EQ(status, easynav_diagnostic_recovery::RecoveryStatus::RUNNING); - // Movement mitigations propose their command; ControllerNode publishes it. - const auto proposed = - easynav::velocity_command::peek(nav_state, easynav::VelocitySource::TAKEOVER); - ASSERT_TRUE(proposed.has_value()); - const auto & cmd = *proposed; - EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.4); - EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.0); + ASSERT_TRUE(catcher.wait_for(1)); + const auto & pose = catcher.poses[0]; + EXPECT_EQ(pose.header.frame_id, "map"); + EXPECT_DOUBLE_EQ(pose.pose.pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(pose.pose.pose.position.y, -1.5); + EXPECT_NEAR(yaw_of(pose.pose.pose.orientation), 0.7, 1e-9); + for (std::size_t i = 0; i < 36; ++i) { + const double expected = (i == 0 || i == 7) ? 0.25 : (i == 35 ? 0.06853891945200942 : 0.0); + EXPECT_DOUBLE_EQ(pose.pose.covariance[i], expected) << "covariance[" << i << "]"; + } } -TEST_F(AmclRelocalizeMitigationTestCase, SucceedsOnceCovarianceDropsBelowThreshold) +TEST_F(AmclRelocalizeMitigationTestCase, ConvergenceIsNotCheckedRightAfterAReset) { auto node = std::make_shared( - "test_succeed_node", - rclcpp::NodeOptions().append_parameter_override("amcl_mit3.covariance_threshold", 0.5)); + "test_settle_node", options("amcl_mit3", 5, 0.3)); auto mit = make_mitigation(node, "amcl_mit3"); easynav::NavState nav_state; - nav_state.set("localizer.amcl.covariance_trace", 0.1); // relocalized + set_robot_pose(nav_state, 0.0, 0.0, 0.0); + set_localization(nav_state, 5.0, 0.1); + mit->internal_start(nav_state); + + // Right after the reset the cloud is still spread: its first scores may look converged + set_localization(nav_state, 0.57, 0.6); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::RUNNING); + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::RUNNING); + // Within the attempt, once it has had time to correct + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::SUCCEEDED); + expect_still(nav_state); +} + +TEST_F(AmclRelocalizeMitigationTestCase, AlreadyConvergedAtStartResetsNothing) +{ + // Selected on a diagnostic evaluated before AMCL converged + auto node = std::make_shared( + "test_converged_node", options("amcl_mit15", 5, 3.0)); + auto mit = make_mitigation(node, "amcl_mit15"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_robot_pose(nav_state, 1.0, 2.0, 0.0); + set_localization(nav_state, 0.05, 0.7); + nav_state.set("localizer.amcl.quality_ratio", 1.0); + + mit->internal_start(nav_state); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::SUCCEEDED); + expect_still(nav_state); + EXPECT_TRUE(catcher.wait_for(0, 0.3)); + + // Next time it is lost: it resets + set_localization(nav_state, 0.05, 0.2); + mit->internal_start(nav_state); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::RUNNING); + ASSERT_TRUE(catcher.wait_for(1)); +} + +TEST_F(AmclRelocalizeMitigationTestCase, TheQualityRatioMustAlsoBeSteady) +{ + auto node = std::make_shared( + "test_ratio_node", + options("amcl_mit16", 5, 0.05) + .append_parameter_override("amcl_mit16.quality_ratio_threshold", 0.6)); + auto mit = make_mitigation(node, "amcl_mit16"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_robot_pose(nav_state, 0.0, 0.0, 0.0); + set_localization(nav_state, 0.05, 0.7); + nav_state.set("localizer.amcl.quality_ratio", 0.3); // the scores just dropped: kidnapped + + // Not converged at start: it resets, and does not succeed while the ratio stays low mit->internal_start(nav_state); - auto status = mit->internal_cycle(nav_state); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); + ASSERT_TRUE(catcher.wait_for(5)); - EXPECT_EQ(status, easynav_diagnostic_recovery::RecoveryStatus::SUCCEEDED); - // Movement mitigations propose their command; ControllerNode publishes it. - const auto proposed = - easynav::velocity_command::peek(nav_state, easynav::VelocitySource::TAKEOVER); - ASSERT_TRUE(proposed.has_value()); - const auto & cmd = *proposed; - EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.0); + nav_state.set("localizer.amcl.quality_ratio", 0.3); + set_localization(nav_state, 5.0, 0.1); + mit->internal_start(nav_state); + nav_state.set("localizer.amcl.quality_ratio", 1.0); // after the reset + set_localization(nav_state, 0.05, 0.7); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::SUCCEEDED); + + // 0: not checked + auto node2 = std::make_shared( + "test_ratio_off_node", + options("amcl_mit17", 5, 0.05) + .append_parameter_override("amcl_mit17.quality_ratio_threshold", 0.0)); + auto mit2 = make_mitigation(node2, "amcl_mit17"); + nav_state.set("localizer.amcl.quality_ratio", 0.1); + mit2->internal_start(nav_state); + EXPECT_EQ(mit2->internal_cycle(nav_state), RecoveryStatus::SUCCEEDED); } -TEST_F(AmclRelocalizeMitigationTestCase, FailsAfterTimeoutWithoutRelocalizing) +TEST_F(AmclRelocalizeMitigationTestCase, ConvergenceIsCheckedUntilTheEndOfTheAttempt) { + // The default settle time (1 s) is within a 3 s attempt auto node = std::make_shared( - "test_timeout_node", - rclcpp::NodeOptions().append_parameter_override("amcl_mit4.timeout", 0.05)); + "test_window_node", options("amcl_mit4", 2, 3.0)); auto mit = make_mitigation(node, "amcl_mit4"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_robot_pose(nav_state, 1.0, 1.0, 0.0); + set_localization(nav_state, 5.0, 0.1); + + mit->internal_start(nav_state); + std::this_thread::sleep_for(std::chrono::milliseconds(1500)); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::RUNNING); + + set_localization(nav_state, 0.2, 0.8); // converges 1.5 s into the attempt + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::SUCCEEDED); + ASSERT_TRUE(catcher.wait_for(1)); // no other attempt +} + +TEST_F(AmclRelocalizeMitigationTestCase, NextAttemptsAreOffsetUpToMaxOffset) +{ + auto node = std::make_shared( + "test_offset_node", options("amcl_mit5", 5, 0.05, 0.4)); + auto mit = make_mitigation(node, "amcl_mit5"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_robot_pose(nav_state, 3.0, 4.0, -2.0); + set_localization(nav_state, 5.0, 0.1); // never converges + + mit->internal_start(nav_state); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); + expect_still(nav_state); + + ASSERT_TRUE(catcher.wait_for(5)); + EXPECT_DOUBLE_EQ(catcher.poses[0].pose.pose.position.x, 3.0); + EXPECT_DOUBLE_EQ(catcher.poses[0].pose.pose.position.y, 4.0); + for (std::size_t i = 1; i < catcher.poses.size(); ++i) { + const auto & p = catcher.poses[i].pose; + const double offset = std::hypot(p.pose.position.x - 3.0, p.pose.position.y - 4.0); + EXPECT_GT(offset, 0.0) << "attempt " << i + 1; + EXPECT_LE(offset, 0.4 + 1e-9) << "attempt " << i + 1; + EXPECT_NEAR(yaw_of(p.pose.orientation), -2.0, 1e-9) << "attempt " << i + 1; + EXPECT_DOUBLE_EQ(p.covariance[0], 0.25); + EXPECT_DOUBLE_EQ(p.covariance[35], 0.06853891945200942); + } +} + +TEST_F(AmclRelocalizeMitigationTestCase, OffsetsSpreadOverTheWholeDisc) +{ + // Many attempts: every offset within max_offset, some near it and in every quadrant + auto node = std::make_shared( + "test_spread_node", options("amcl_mit6", 200, 0.001, 1.0)); + auto mit = make_mitigation(node, "amcl_mit6"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_robot_pose(nav_state, 0.0, 0.0, 0.0); + set_localization(nav_state, 5.0); + + mit->internal_start(nav_state); + EXPECT_EQ(run(mit, nav_state, 20.0, &catcher), RecoveryStatus::FAILED); + ASSERT_TRUE(catcher.wait_for(200, 5.0)); + + double farthest = 0.0; + int quadrants[4] = {0, 0, 0, 0}; + for (std::size_t i = 1; i < catcher.poses.size(); ++i) { + const auto & p = catcher.poses[i].pose.pose.position; + const double r = std::hypot(p.x, p.y); + EXPECT_LE(r, 1.0 + 1e-9); + farthest = std::max(farthest, r); + ++quadrants[(p.x >= 0.0 ? 0 : 1) + (p.y >= 0.0 ? 0 : 2)]; + } + EXPECT_GT(farthest, 0.8); + for (int q : quadrants) { + EXPECT_GT(q, 0); + } +} + +TEST_F(AmclRelocalizeMitigationTestCase, ZeroMaxOffsetRepeatsTheLostPose) +{ + auto node = std::make_shared( + "test_zero_offset_node", options("amcl_mit7", 3, 0.05, 0.0)); + auto mit = make_mitigation(node, "amcl_mit7"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_robot_pose(nav_state, -1.0, 2.0, 1.0); + set_localization(nav_state, 5.0); + + mit->internal_start(nav_state); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); + ASSERT_TRUE(catcher.wait_for(3)); + for (const auto & pose : catcher.poses) { + EXPECT_DOUBLE_EQ(pose.pose.pose.position.x, -1.0); + EXPECT_DOUBLE_EQ(pose.pose.pose.position.y, 2.0); + } +} + +TEST_F(AmclRelocalizeMitigationTestCase, OneAttemptOnlyTriesTheLostPose) +{ + auto node = std::make_shared( + "test_one_node", options("amcl_mit8", 1, 0.05)); + auto mit = make_mitigation(node, "amcl_mit8"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_robot_pose(nav_state, 0.5, 0.5, 0.0); + set_localization(nav_state, 5.0); + + mit->internal_start(nav_state); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); + ASSERT_TRUE(catcher.wait_for(1)); +} + +TEST_F(AmclRelocalizeMitigationTestCase, SucceedsOnALaterAttempt) +{ + auto node = std::make_shared( + "test_later_node", options("amcl_mit9", 5, 0.1)); + auto mit = make_mitigation(node, "amcl_mit9"); + PoseCatcher catcher; + catcher.wait_matched(node); easynav::NavState nav_state; - nav_state.set("localizer.amcl.covariance_trace", 5.0); // still diverged + set_robot_pose(nav_state, 0.0, 0.0, 0.0); + set_localization(nav_state, 5.0, 0.1); mit->internal_start(nav_state); - std::this_thread::sleep_for(std::chrono::milliseconds(60)); // past the 50 ms timeout - auto status = mit->internal_cycle(nav_state); + // The first attempt times out: the second one starts + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::RUNNING); + ASSERT_TRUE(catcher.wait_for(2)); - EXPECT_EQ(status, easynav_diagnostic_recovery::RecoveryStatus::FAILED); - // Movement mitigations propose their command; ControllerNode publishes it. - const auto proposed = - easynav::velocity_command::peek(nav_state, easynav::VelocitySource::TAKEOVER); - ASSERT_TRUE(proposed.has_value()); - const auto & cmd = *proposed; - EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.0); + set_localization(nav_state, 0.2, 0.8); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::SUCCEEDED); + EXPECT_TRUE(catcher.wait_for(2, 0.2)); // no third attempt } TEST_F(AmclRelocalizeMitigationTestCase, ACompactCloudMustAlsoMatchTheMap) { auto node = std::make_shared( "test_match_node", - rclcpp::NodeOptions() - .append_parameter_override("amcl_mit5.covariance_threshold", 0.5) - .append_parameter_override("amcl_mit5.match_threshold", 0.3)); - auto mit = make_mitigation(node, "amcl_mit5"); + options("amcl_mit10", 5, 0.05) + .append_parameter_override("amcl_mit10.covariance_threshold", 0.5) + .append_parameter_override("amcl_mit10.match_threshold", 0.3)); + auto mit = make_mitigation(node, "amcl_mit10"); easynav::NavState nav_state; - nav_state.set("localizer.amcl.covariance_trace", 0.1); - nav_state.set("localizer.amcl.match_score", 0.1); // compact, but lost + set_robot_pose(nav_state, 0.0, 0.0, 0.0); + set_localization(nav_state, 0.1, 0.1); // compact, but lost mit->internal_start(nav_state); - EXPECT_EQ( - mit->internal_cycle(nav_state), easynav_diagnostic_recovery::RecoveryStatus::RUNNING); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); - nav_state.set("localizer.amcl.match_score", 0.5); - EXPECT_EQ( - mit->internal_cycle(nav_state), easynav_diagnostic_recovery::RecoveryStatus::SUCCEEDED); + set_localization(nav_state, 5.0, 0.1); + mit->internal_start(nav_state); + set_localization(nav_state, 0.1, 0.5); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::SUCCEEDED); + + set_localization(nav_state, 5.0, 0.1); + mit->internal_start(nav_state); + set_localization(nav_state, 0.8, 0.9); // matches, but spread + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); } TEST_F(AmclRelocalizeMitigationTestCase, WithoutAMatchScoreOnlyTheCovarianceCounts) { auto node = std::make_shared( "test_no_match_node", - rclcpp::NodeOptions() - .append_parameter_override("amcl_mit6.covariance_threshold", 0.5) - .append_parameter_override("amcl_mit6.match_threshold", 0.3)); - auto mit = make_mitigation(node, "amcl_mit6"); + options("amcl_mit11", 5, 0.05) + .append_parameter_override("amcl_mit11.covariance_threshold", 0.5) + .append_parameter_override("amcl_mit11.match_threshold", 0.3)); + auto mit = make_mitigation(node, "amcl_mit11"); easynav::NavState nav_state; - nav_state.set("localizer.amcl.covariance_trace", 0.1); + set_robot_pose(nav_state, 0.0, 0.0, 0.0); + set_localization(nav_state, 5.0); + mit->internal_start(nav_state); + set_localization(nav_state, 0.1); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::SUCCEEDED); +} + +TEST_F(AmclRelocalizeMitigationTestCase, WithoutACovarianceItDoesNotConverge) +{ + auto node = std::make_shared( + "test_no_trace_node", options("amcl_mit12", 2, 0.05)); + auto mit = make_mitigation(node, "amcl_mit12"); + + easynav::NavState nav_state; + set_robot_pose(nav_state, 0.0, 0.0, 0.0); + mit->internal_start(nav_state); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); +} + +TEST_F(AmclRelocalizeMitigationTestCase, WithoutARobotPoseItFailsWithoutResetting) +{ + auto node = std::make_shared( + "test_no_pose_node", options("amcl_mit13", 5, 3.0)); + auto mit = make_mitigation(node, "amcl_mit13"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_localization(nav_state, 5.0); + mit->internal_start(nav_state); + EXPECT_EQ(mit->internal_cycle(nav_state), RecoveryStatus::FAILED); + expect_still(nav_state); + EXPECT_TRUE(catcher.wait_for(0, 0.2)); +} + +TEST_F(AmclRelocalizeMitigationTestCase, EachRecoveryStartsFromTheRobotPoseThen) +{ + auto node = std::make_shared( + "test_restart_node", options("amcl_mit14", 2, 0.05, 0.0)); + auto mit = make_mitigation(node, "amcl_mit14"); + PoseCatcher catcher; + catcher.wait_matched(node); + + easynav::NavState nav_state; + set_robot_pose(nav_state, 1.0, 0.0, 0.0); + set_localization(nav_state, 5.0); + mit->internal_start(nav_state); + set_robot_pose(nav_state, 9.0, 9.0, 0.0); // the estimate moves during the recovery: ignored + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); + mit->internal_stop(nav_state); + ASSERT_TRUE(catcher.wait_for(2)); + EXPECT_DOUBLE_EQ(catcher.poses[1].pose.pose.position.x, 1.0); + + // A new recovery: from where the robot is now, all its attempts again + set_robot_pose(nav_state, -3.0, 2.0, 0.0); mit->internal_start(nav_state); - EXPECT_EQ( - mit->internal_cycle(nav_state), easynav_diagnostic_recovery::RecoveryStatus::SUCCEEDED); + EXPECT_EQ(run(mit, nav_state), RecoveryStatus::FAILED); + ASSERT_TRUE(catcher.wait_for(4)); + EXPECT_DOUBLE_EQ(catcher.poses[2].pose.pose.position.x, -3.0); + EXPECT_DOUBLE_EQ(catcher.poses[3].pose.pose.position.y, 2.0); } diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_reseed_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_reseed_tests.cpp index f530890e..67412c96 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_reseed_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_reseed_tests.cpp @@ -28,6 +28,8 @@ #include "gtest/gtest.h" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + #include "rclcpp/rclcpp.hpp" #include "easynav_common/RTTFBuffer.hpp" @@ -44,6 +46,7 @@ namespace class TestAMCLLocalizer : public easynav::AMCLLocalizer { public: + using easynav::AMCLLocalizer::init_pose_callback; using easynav::AMCLLocalizer::odom_callback; using easynav::AMCLLocalizer::particles_; using easynav::AMCLLocalizer::reseed; @@ -203,3 +206,124 @@ TEST_F(CostmapAMCLReseedTest, ReseedWithAlikeWeightsDoesNotSpreadTheCloud) } EXPECT_LT(std_xy(), 2.0 * before) << "before " << before << " after " << std_xy(); } + +namespace +{ + +// RViz2's "2D Pose Estimate" at (1, 2, 0) +std::unique_ptr rviz_pose() +{ + auto msg = std::make_unique(); + msg->header.frame_id = "map"; + msg->pose.pose.position.x = 1.0; + msg->pose.pose.position.y = 2.0; + msg->pose.pose.orientation.w = 1.0; + msg->pose.covariance[0] = 0.25; + msg->pose.covariance[7] = 0.25; + msg->pose.covariance[35] = 0.06853891945200942; + return msg; +} + +// Collapses the cloud on (1, 2, 0), all alike, reseeds it and returns the deviation of the new +// particles from there (xy, yaw) +std::pair reseed_spread(TestAMCLLocalizer & localizer) +{ + auto & ps = localizer.particles_; + for (auto & p : ps) { + p.pose.setIdentity(); + p.pose.setOrigin(tf2::Vector3(1.0, 2.0, 0.0)); + p.hits = 10; + p.possible_hits = 20; + p.weight = 1.0 / static_cast(ps.size()); + } + localizer.reseed(); + double xy = 0.0, yaw = 0.0; + const std::size_t from = ps.size() / 2; // the reseeded ones + for (std::size_t i = from; i < ps.size(); ++i) { + const auto o = ps[i].pose.getOrigin(); + xy += 0.5 * (std::pow(o.x() - 1.0, 2) + std::pow(o.y() - 2.0, 2)); + double r, pi, y; + tf2::Matrix3x3(ps[i].pose.getRotation()).getRPY(r, pi, y); + yaw += y * y; + } + const double n = static_cast(ps.size() - from); + return {std::sqrt(xy / n), std::sqrt(yaw / n)}; +} + +} // namespace + +// Standing still there is no motion noise: after an initial pose the reseeds spread the new +// particles more, so that the cloud keeps looking for the pose that matches best, falling to +// min_noise by the end of reset_refine_time. Only after an initial pose. +TEST_F(CostmapAMCLReseedTest, AfterAnInitialPoseTheReseedsRefineItForAWhile) +{ + auto node = std::make_shared(); + const std::string name = "refine_localizer"; + node->declare_parameter(name + ".num_particles", 2000); + node->declare_parameter(name + ".min_noise_xy", 0.01); + node->declare_parameter(name + ".min_noise_yaw", 0.01); + node->declare_parameter(name + ".reset_refine_time", 0.6); + node->declare_parameter(name + ".reset_refine_noise", 0.25); + auto localizer = std::make_shared(); + localizer->initialize(node, name); + + // Before any initial pose: min_noise + auto [xy, yaw] = reseed_spread(*localizer); + EXPECT_NEAR(xy, 0.01, 0.002); + EXPECT_NEAR(yaw, 0.01, 0.002); + + // Right after it: a quarter of its deviations (0.5 m, pi/12 rad) + localizer->init_pose_callback(rviz_pose()); + std::tie(xy, yaw) = reseed_spread(*localizer); + EXPECT_NEAR(xy, 0.125, 0.02); + EXPECT_NEAR(yaw, 0.0654, 0.01); + + // Halfway: about half of it + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + std::tie(xy, yaw) = reseed_spread(*localizer); + EXPECT_GT(xy, 0.04); + EXPECT_LT(xy, 0.085); + + // Over: min_noise again + std::this_thread::sleep_for(std::chrono::milliseconds(400)); + std::tie(xy, yaw) = reseed_spread(*localizer); + EXPECT_NEAR(xy, 0.01, 0.002); + EXPECT_NEAR(yaw, 0.01, 0.002); + + // A new initial pose refines again + localizer->init_pose_callback(rviz_pose()); + std::tie(xy, yaw) = reseed_spread(*localizer); + EXPECT_NEAR(xy, 0.125, 0.02); +} + +TEST_F(CostmapAMCLReseedTest, TheRefinementCanBeTurnedOff) +{ + for (const auto & [time, noise] : {std::pair{0.0, 0.25}, std::pair{3.0, 0.0}}) { + auto node = std::make_shared(); + const std::string name = "no_refine_localizer"; + node->declare_parameter(name + ".num_particles", 2000); + node->declare_parameter(name + ".min_noise_xy", 0.01); + node->declare_parameter(name + ".min_noise_yaw", 0.01); + node->declare_parameter(name + ".reset_refine_time", time); + node->declare_parameter(name + ".reset_refine_noise", noise); + auto localizer = std::make_shared(); + localizer->initialize(node, name); + localizer->init_pose_callback(rviz_pose()); + const auto [xy, yaw] = reseed_spread(*localizer); + EXPECT_NEAR(xy, 0.01, 0.002) << "time " << time << " noise " << noise; + EXPECT_NEAR(yaw, 0.01, 0.002) << "time " << time << " noise " << noise; + } +} + +TEST_F(CostmapAMCLReseedTest, NegativeRefinementIsRejected) +{ + for (const auto & [param, value] : {std::pair{"reset_refine_time", -1.0}, + std::pair{"reset_refine_noise", -0.1}}) + { + auto node = std::make_shared(); + const std::string name = "bad_refine_localizer"; + node->declare_parameter(name + "." + param, value); + auto localizer = std::make_shared(); + EXPECT_ANY_THROW(localizer->initialize(node, name)) << param; + } +} diff --git a/recoveries/easynav_diagnostic_recovery/README.md b/recoveries/easynav_diagnostic_recovery/README.md index c395a33a..aeec346a 100644 --- a/recoveries/easynav_diagnostic_recovery/README.md +++ b/recoveries/easynav_diagnostic_recovery/README.md @@ -71,7 +71,7 @@ Each plugin's parameters are under `recovery_manager..`. Every mitigation | `easynav_diagnostic_recovery/HumanAssistanceRecovery` | Handles any `ERROR` not in `ignored_hardware_ids`: stops and waits until those clear. `timeout` 0 waits forever. | `timeout` (0.0), `ignored_hardware_ids` ([`ros_graph`]) | | `easynav_diagnostic_recovery/CancelMissionRecovery` | Handles any `ERROR`: aborts the mission. The last resort. | — | | `easynav_costmap_localizer/AmclConvergenceEvaluator` | `ERROR` (`localizer.amcl`) if AMCL's covariance trace exceeds the threshold. | `covariance_threshold` (1.0) | -| `easynav_costmap_localizer/AmclRelocalizeMitigation` | Handles `localizer.amcl`: rotates until AMCL converges, or fails after `timeout`. | `covariance_threshold` (1.0), `rotation_speed` (0.3), `timeout` (5.0) | +| `easynav_costmap_localizer/AmclRelocalizeMitigation` | Handles `localizer.amcl`: holds the robot and resets AMCL's pose (on `initialpose`, with RViz2's covariance) to where the robot was when it got lost, then up to `max_offset` m from it. Each of the `attempts` has `attempt_timeout` s to converge, by the evaluator's criteria; fails if none does. Resets nothing if AMCL has already converged when it starts. | `attempts` (5), `attempt_timeout` (5.0), `max_offset` (1.0), `covariance_threshold` (1.0), `match_threshold` (0.5), `quality_ratio_threshold` (0.5) | ## Usage