diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index c44afdb9..c00357b4 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/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 $ 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 a88178d7..486e7160 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/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 tf_broadcaster_; diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp index 790b9c56..18be6c6d 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp +++ b/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 + #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 low_since_; }; } // namespace easynav 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 f3f73819..34ef0060 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp +++ b/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_; }; diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/LocalizationQuality.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/LocalizationQuality.hpp new file mode 100644 index 00000000..b3ff7a2a --- /dev/null +++ b/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 + +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_ 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 770c8944..48d287f6 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -247,6 +247,19 @@ 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_); + double alpha_slow = 0.001, alpha_fast = 0.1; + easynav::declare_parameter_if_absent( + *node, plugin_name + ".recovery_alpha_slow", alpha_slow); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".recovery_alpha_fast", alpha_fast); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".random_particles_max_fraction", random_particles_max_fraction_); + node->get_parameter(plugin_name + ".recovery_alpha_slow", alpha_slow); + node->get_parameter(plugin_name + ".recovery_alpha_fast", alpha_fast); + node->get_parameter( + 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(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); node->get_parameter(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("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(hits) / static_cast(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(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(particles_.size()); + const double share = std::min(quality_.injection_probability(), random_particles_max_fraction_); + return static_cast(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 cell_x(0, map->getSizeInCellsX() - 1); + std::uniform_int_distribution cell_y(0, map->getSizeInCellsY() - 1); + std::uniform_real_distribution 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; diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp index f75d4294..b5a06cb1 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp +++ b/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(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_); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".debounce_duration", debounce_duration_); + node->get_parameter(plugin_name + ".match_threshold", match_threshold_); + node->get_parameter(plugin_name + ".quality_ratio_threshold", quality_ratio_threshold_); + node->get_parameter(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("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("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("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); 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 8e9aefd1..d3700c24 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp @@ -41,6 +41,9 @@ void AmclRelocalizeMitigation::on_initialize() 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_); + easynav::declare_parameter_if_absent( + *node, plugin_name + ".match_threshold", match_threshold_); + node->get_parameter(plugin_name + ".match_threshold", match_threshold_); } bool AmclRelocalizeMitigation::can_handle( @@ -66,7 +69,11 @@ easynav_diagnostic_recovery::RecoveryStatus AmclRelocalizeMitigation::on_cycle(N // 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"); - if (trace <= covariance_threshold_) { + // 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; } diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/LocalizationQuality.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/LocalizationQuality.cpp new file mode 100644 index 00000000..2a257170 --- /dev/null +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/LocalizationQuality.cpp @@ -0,0 +1,66 @@ +// 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. + +#include "easynav_costmap_localizer/LocalizationQuality.hpp" + +#include +#include + +namespace easynav +{ + +LocalizationQuality::LocalizationQuality(double alpha_slow, double alpha_fast) +: alpha_slow_(std::clamp(alpha_slow, 0.0, 1.0)), alpha_fast_(std::clamp(alpha_fast, 0.0, 1.0)) +{ +} + +void LocalizationQuality::add(double mean_score, double best_score) +{ + if (!std::isfinite(mean_score) || !std::isfinite(best_score)) { + return; + } + ++count_; + has_data_ = true; + // The plain mean of the scores so far until it weighs less than alpha. + const double warm = 1.0 / static_cast(count_); + const double a_fast = std::max(alpha_fast_ > 0.0 ? alpha_fast_ : 1.0, warm); + best_ += a_fast * (best_score - best_); + if (averaging()) { + slow_ += std::max(alpha_slow_, warm) * (mean_score - slow_); + fast_ += a_fast * (mean_score - fast_); + } +} + +double LocalizationQuality::quality_ratio() const +{ + if (!has_data_ || !averaging() || slow_ <= 1e-9) { + return 1.0; + } + return fast_ / slow_; +} + +double LocalizationQuality::injection_probability() const +{ + return std::max(0.0, 1.0 - quality_ratio()); +} + +void LocalizationQuality::reset() +{ + has_data_ = false; + count_ = 0; + best_ = slow_ = fast_ = 0.0; +} + +} // namespace easynav diff --git a/localizers/easynav_costmap_localizer/tests/CMakeLists.txt b/localizers/easynav_costmap_localizer/tests/CMakeLists.txt index 6f5903ab..dfcecaf5 100644 --- a/localizers/easynav_costmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/tests/CMakeLists.txt @@ -39,3 +39,9 @@ target_link_libraries(costmap_localizer_min_height_tests ${PROJECT_NAME}) ament_add_gtest(costmap_localizer_reseed_tests costmap_localizer_reseed_tests.cpp TIMEOUT 120) target_link_libraries(costmap_localizer_reseed_tests ${PROJECT_NAME}) + +ament_add_gtest(costmap_localizer_quality_tests costmap_localizer_quality_tests.cpp) +target_link_libraries(costmap_localizer_quality_tests + ${PROJECT_NAME} + rclcpp::rclcpp +) diff --git a/localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp b/localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp index 0c5601fa..b7c3c54e 100644 --- a/localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include #include "gtest/gtest.h" @@ -88,3 +90,135 @@ TEST_F(AmclConvergenceEvaluatorTestCase, ErrorWhenCovarianceAboveThreshold) EXPECT_EQ(status.values[0].key, "covariance_trace"); EXPECT_NEAR(std::stod(status.values[0].value), 2.5, 1e-3); } + +namespace +{ + +std::shared_ptr scored_node( + const std::string & node_name, const std::string & plugin, double match_threshold) +{ + return std::make_shared( + node_name, + rclcpp::NodeOptions() + .append_parameter_override(plugin + ".match_threshold", match_threshold) + .append_parameter_override(plugin + ".debounce_duration", 0.3) + .append_parameter_override(plugin + ".freq", 50.0)); +} + +const diagnostic_msgs::msg::DiagnosticStatus & status_of( + easynav::NavState & nav_state, const std::string & plugin) +{ + return nav_state.get("diagnostics." + plugin); +} + +void step(easynav::AmclConvergenceEvaluator & eval, easynav::NavState & nav_state) +{ + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + eval.internal_update(nav_state); +} + +} // namespace + +TEST_F(AmclConvergenceEvaluatorTestCase, ACompactCloudThatDoesNotMatchTheMapIsLost) +{ + auto node = scored_node("test_lost_node", "amcl4", 0.3); + auto eval = make_ready_evaluator(node, "amcl4"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 0.01); // compact + nav_state.set("localizer.amcl.match_score", 0.05); // but wrong + nav_state.set("localizer.amcl.quality_ratio", 1.0); + step(*eval, nav_state); + EXPECT_EQ(status_of(nav_state, "amcl4").level, diagnostic_msgs::msg::DiagnosticStatus::OK) + << "not before the debounce"; + step(*eval, nav_state); + step(*eval, nav_state); + step(*eval, nav_state); + const auto status = status_of(nav_state, "amcl4"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_NE(status.message.find("lost"), std::string::npos) << status.message; + bool has_match = false; + for (const auto & kv : status.values) { + has_match = has_match || kv.key == "match_score"; + } + EXPECT_TRUE(has_match); +} + +TEST_F(AmclConvergenceEvaluatorTestCase, AGoodMatchClearsItAndRestartsTheDebounce) +{ + auto node = scored_node("test_clear_node", "amcl5", 0.3); + auto eval = make_ready_evaluator(node, "amcl5"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 0.01); + nav_state.set("localizer.amcl.match_score", 0.05); + for (int i = 0; i < 4; ++i) { + step(*eval, nav_state); + } + EXPECT_EQ(status_of(nav_state, "amcl5").level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + + nav_state.set("localizer.amcl.match_score", 0.6); + step(*eval, nav_state); + EXPECT_EQ(status_of(nav_state, "amcl5").level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + // A short dip is not an error. + nav_state.set("localizer.amcl.match_score", 0.1); + step(*eval, nav_state); + EXPECT_EQ(status_of(nav_state, "amcl5").level, diagnostic_msgs::msg::DiagnosticStatus::OK); + nav_state.set("localizer.amcl.match_score", 0.6); + step(*eval, nav_state); + step(*eval, nav_state); + step(*eval, nav_state); + EXPECT_EQ(status_of(nav_state, "amcl5").level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(AmclConvergenceEvaluatorTestCase, AMatchThresholdOfZeroDoesNotCheckTheMatch) +{ + auto node = scored_node("test_disabled_node", "amcl6", 0.0); + auto eval = make_ready_evaluator(node, "amcl6"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 0.01); + nav_state.set("localizer.amcl.match_score", 0.0); + nav_state.set("localizer.amcl.quality_ratio", 1.0); + for (int i = 0; i < 5; ++i) { + step(*eval, nav_state); + } + EXPECT_EQ(status_of(nav_state, "amcl6").level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(AmclConvergenceEvaluatorTestCase, ASuddenDropOfTheMatchIsAKidnapping) +{ + auto node = scored_node("test_drop_node", "amcl7", 0.0); + auto eval = make_ready_evaluator(node, "amcl7"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 0.01); + nav_state.set("localizer.amcl.match_score", 0.5); + nav_state.set("localizer.amcl.quality_ratio", 0.2); + for (int i = 0; i < 4; ++i) { + step(*eval, nav_state); + } + const auto status = status_of(nav_state, "amcl7"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_NE(status.message.find("dropped"), std::string::npos) << status.message; + + nav_state.set("localizer.amcl.quality_ratio", 0.9); + step(*eval, nav_state); + EXPECT_EQ(status_of(nav_state, "amcl7").level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(AmclConvergenceEvaluatorTestCase, ASpreadCloudIsStillAnErrorAtOnce) +{ + auto node = scored_node("test_spread_node", "amcl8", 0.3); + auto eval = make_ready_evaluator(node, "amcl8"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 5.0); + nav_state.set("localizer.amcl.match_score", 0.9); + nav_state.set("localizer.amcl.quality_ratio", 1.0); + step(*eval, nav_state); + const auto status = status_of(nav_state, "amcl8"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(status.message, "particle filter diverged"); +} 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 15e3d1bd..d0a876bf 100644 --- a/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp @@ -141,3 +141,41 @@ TEST_F(AmclRelocalizeMitigationTestCase, FailsAfterTimeoutWithoutRelocalizing) const auto & cmd = *proposed; EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.0); } + +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"); + + 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 + + mit->internal_start(nav_state); + EXPECT_EQ( + mit->internal_cycle(nav_state), easynav_diagnostic_recovery::RecoveryStatus::RUNNING); + + nav_state.set("localizer.amcl.match_score", 0.5); + EXPECT_EQ( + mit->internal_cycle(nav_state), easynav_diagnostic_recovery::RecoveryStatus::SUCCEEDED); +} + +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"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 0.1); + mit->internal_start(nav_state); + EXPECT_EQ( + mit->internal_cycle(nav_state), easynav_diagnostic_recovery::RecoveryStatus::SUCCEEDED); +} diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_quality_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_quality_tests.cpp new file mode 100644 index 00000000..e8f3cb55 --- /dev/null +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_quality_tests.cpp @@ -0,0 +1,320 @@ +// 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. + +/// \brief LocalizationQuality, and the AMCL's scores and random particles on a map. + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" + +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_costmap_common/cost_values.hpp" +#include "easynav_costmap_common/costmap_2d.hpp" +#include "easynav_costmap_localizer/AMCLLocalizer.hpp" +#include "easynav_costmap_localizer/LocalizationQuality.hpp" +#include "easynav_localizer/LocalizerNode.hpp" +#include "easynav_sensors/types/PointPerception.hpp" + +namespace +{ + +using easynav::LocalizationQuality; + +TEST(LocalizationQuality, NoDataMeansNoDrop) +{ + LocalizationQuality q; + EXPECT_FALSE(q.has_data()); + EXPECT_DOUBLE_EQ(q.quality_ratio(), 1.0); + EXPECT_DOUBLE_EQ(q.injection_probability(), 0.0); +} + +TEST(LocalizationQuality, TheFirstScoresSetTheLevel) +{ + LocalizationQuality q(0.01, 0.2); + q.add(0.4, 0.7); + EXPECT_TRUE(q.has_data()); + EXPECT_DOUBLE_EQ(q.match_score(), 0.7); + EXPECT_DOUBLE_EQ(q.slow(), 0.4); + EXPECT_DOUBLE_EQ(q.fast(), 0.4); + EXPECT_DOUBLE_EQ(q.quality_ratio(), 1.0); +} + +TEST(LocalizationQuality, TheUnconvergedStartDoesNotSetTheLevel) +{ + LocalizationQuality q(0.001, 0.1); + for (int i = 0; i < 5; ++i) { + q.add(0.05, 0.1); // the initial cloud, not yet on the map + } + for (int i = 0; i < 300; ++i) { + q.add(0.5, 0.75); // converged + } + // The slow average is the mean so far, not the first value: the ratio is near 1, not 10. + EXPECT_NEAR(q.slow(), (5 * 0.05 + 300 * 0.5) / 305.0, 0.02); + EXPECT_NEAR(q.quality_ratio(), 1.0, 0.05); + // So a drop shows clearly. + for (int i = 0; i < 20; ++i) { + q.add(0.1, 0.25); + } + EXPECT_LT(q.quality_ratio(), 0.35); +} + +TEST(LocalizationQuality, ASteadyMatchKeepsTheRatioAtOne) +{ + LocalizationQuality q(0.01, 0.2); + for (int i = 0; i < 500; ++i) { + q.add(0.4, 0.7); + } + EXPECT_NEAR(q.quality_ratio(), 1.0, 1e-9); + EXPECT_NEAR(q.injection_probability(), 0.0, 1e-9); + EXPECT_NEAR(q.match_score(), 0.7, 1e-9); +} + +TEST(LocalizationQuality, ADropLowersTheRatioAndAskForRandomParticles) +{ + LocalizationQuality q(0.01, 0.2); + for (int i = 0; i < 200; ++i) { + q.add(0.4, 0.7); + } + for (int i = 0; i < 20; ++i) { + q.add(0.05, 0.1); // kidnapped + } + EXPECT_LT(q.quality_ratio(), 0.3); + EXPECT_GT(q.injection_probability(), 0.7); + EXPECT_LT(q.match_score(), 0.15); + // If it stays there, the slow average follows: the drop is no longer news. + for (int i = 0; i < 3000; ++i) { + q.add(0.05, 0.1); + } + EXPECT_NEAR(q.quality_ratio(), 1.0, 0.01); + EXPECT_LT(q.match_score(), 0.15) << "but the match is still low"; +} + +TEST(LocalizationQuality, ABetterMatchIsNoDrop) +{ + LocalizationQuality q(0.01, 0.2); + q.add(0.2, 0.3); + for (int i = 0; i < 20; ++i) { + q.add(0.6, 0.9); + } + EXPECT_GT(q.quality_ratio(), 1.0); + EXPECT_DOUBLE_EQ(q.injection_probability(), 0.0); +} + +TEST(LocalizationQuality, ZeroAlphasDisableTheAverages) +{ + for (const auto & [slow, fast] : std::vector>{{0.0, 0.1}, + {0.01, 0.0}}) + { + LocalizationQuality q(slow, fast); + q.add(0.5, 0.8); + q.add(0.01, 0.02); + EXPECT_DOUBLE_EQ(q.quality_ratio(), 1.0); + EXPECT_DOUBLE_EQ(q.injection_probability(), 0.0); + } +} + +TEST(LocalizationQuality, NonFiniteScoresAreIgnoredAndResetForgets) +{ + LocalizationQuality q; + q.add(NAN, 0.5); + EXPECT_FALSE(q.has_data()); + q.add(0.3, 0.6); + q.add(0.3, INFINITY); + EXPECT_DOUBLE_EQ(q.match_score(), 0.6); + q.reset(); + EXPECT_FALSE(q.has_data()); + EXPECT_DOUBLE_EQ(q.quality_ratio(), 1.0); +} + +class TestAMCLLocalizer : public easynav::AMCLLocalizer +{ +public: + using easynav::AMCLLocalizer::correct; + using easynav::AMCLLocalizer::particles_; + using easynav::AMCLLocalizer::quality_; + using easynav::AMCLLocalizer::random_particles_to_inject; + using easynav::AMCLLocalizer::reseed; +}; + +/// 4 x 4 m, free, with a wall along x = 1 m. +easynav::Costmap2D walled_map() +{ + easynav::Costmap2D map(80, 80, 0.05, -2.0, -2.0); + for (unsigned int cy = 0; cy < 80; ++cy) { + unsigned int cx, unused; + map.worldToMap(1.0, 0.0, cx, unused); + map.setCost(cx, cy, easynav::LETHAL_OBSTACLE); + } + return map; +} + +class AMCLScoresTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make(const std::string & name, int n = 100) + { + node_ = std::make_shared(); + node_->declare_parameter(name + ".num_particles", n); + auto localizer = std::make_shared(); + localizer->initialize(node_, name); + return localizer; + } + + /// The wall seen 1 m ahead, from the robot footprint. + void see_the_wall(easynav::NavState & nav_state) + { + easynav::PointPerception perception; + perception.frame_id = + easynav::RTTFBuffer::getInstance()->get_tf_info().robot_footprint_frame; + perception.stamp = node_->now(); + perception.valid = true; + for (int i = -10; i <= 10; ++i) { + perception.data.push_back(pcl::PointXYZ(1.0f, 0.05f * static_cast(i), 0.3f)); + } + nav_state.set("scan", perception); + } + + static void place_all(TestAMCLLocalizer & localizer, double x) + { + for (auto & p : localizer.particles_) { + p.pose.setIdentity(); + p.pose.setOrigin(tf2::Vector3(x, 0.0, 0.0)); + } + } + + std::shared_ptr node_; +}; + +TEST_F(AMCLScoresTest, TheScoresTellARightPoseFromAWrongOne) +{ + auto localizer = make("scores_localizer"); + easynav::NavState nav_state; + nav_state.set("map.base", walled_map()); + see_the_wall(nav_state); + + place_all(*localizer, 0.0); // where the robot is: the wall 1 m ahead + localizer->correct(nav_state); + ASSERT_TRUE(localizer->quality_.has_data()); + EXPECT_GT(localizer->quality_.match_score(), 0.8); + + // All of them 0.5 m off, still a compact cloud: the scan misses the wall. + place_all(*localizer, -0.5); + for (int i = 0; i < 40; ++i) { + localizer->correct(nav_state); + } + EXPECT_LT(localizer->quality_.match_score(), 0.1); + EXPECT_LT(localizer->quality_.quality_ratio(), 0.5) << "a drop from the first level"; + EXPECT_GT(localizer->random_particles_to_inject(), 0u); +} + +TEST_F(AMCLScoresTest, AReseedPlacesTheWorstAtRandomFreePoses) +{ + auto localizer = make("inject_localizer", 100); + const auto map = walled_map(); + // Better half at x = 0, worse half at x = 0.5, told apart by their weights. + for (std::size_t i = 0; i < localizer->particles_.size(); ++i) { + auto & p = localizer->particles_[i]; + p.pose.setIdentity(); + p.pose.setOrigin(tf2::Vector3(i < 50 ? 0.0 : 0.5, 0.0, 0.0)); + p.hits = i < 50 ? 30 : 1; + p.possible_hits = 40; + p.weight = i < 50 ? 0.019 : 0.001; + } + localizer->reseed(&map, 20); + + std::size_t far = 0; + for (const auto & p : localizer->particles_) { + const auto o = p.pose.getOrigin(); + ASSERT_TRUE(std::isfinite(o.x()) && std::isfinite(o.y())); + // Inside the map and never on the wall. + EXPECT_GE(o.x(), -2.0); + EXPECT_LT(o.x(), 2.0); + EXPECT_GE(o.y(), -2.0); + EXPECT_LT(o.y(), 2.0); + unsigned int cx, cy; + ASSERT_TRUE(map.worldToMap(o.x(), o.y(), cx, cy)); + EXPECT_LT(map.getCost(cx, cy), easynav::INSCRIBED_INFLATED_OBSTACLE); + far += std::hypot(o.x(), o.y()) > 0.5; // copies stay within a few cm of x = 0 + } + EXPECT_EQ(localizer->particles_.size(), 100u) << "same population"; + EXPECT_GE(far, 15u) << "about the 20 random ones"; + EXPECT_LE(far, 22u); + double total = 0.0; + for (const auto & p : localizer->particles_) { + total += p.weight; + } + EXPECT_NEAR(total, 1.0, 1e-9); +} + +TEST_F(AMCLScoresTest, NeverMoreRandomOnesThanTheWorseHalf) +{ + auto localizer = make("cap_localizer", 40); + const auto map = walled_map(); + for (auto & p : localizer->particles_) { + p.pose.setIdentity(); + p.hits = 1; + p.possible_hits = 40; + } + localizer->reseed(&map, 1000); + std::size_t at_origin = 0; + for (const auto & p : localizer->particles_) { + at_origin += p.pose.getOrigin().length() < 0.3; + } + EXPECT_GE(at_origin, 20u) << "the better half is kept"; +} + +TEST_F(AMCLScoresTest, WithoutAMapOrADropNoneIsRandom) +{ + auto localizer = make("none_localizer", 50); + for (auto & p : localizer->particles_) { + p.pose.setIdentity(); + p.hits = 10; + p.possible_hits = 40; + } + EXPECT_EQ(localizer->random_particles_to_inject(), 0u) << "no scores yet"; + localizer->reseed(nullptr, 10); + for (const auto & p : localizer->particles_) { + EXPECT_LT(p.pose.getOrigin().length(), 0.5); + } +} + +TEST_F(AMCLScoresTest, TheShareOfRandomOnesIsCapped) +{ + node_ = std::make_shared(); + node_->declare_parameter("capped_localizer.num_particles", 100); + node_->declare_parameter("capped_localizer.random_particles_max_fraction", 0.1); + auto localizer = std::make_shared(); + localizer->initialize(node_, "capped_localizer"); + localizer->quality_.add(0.5, 0.8); + for (int i = 0; i < 50; ++i) { + localizer->quality_.add(0.0, 0.0); + } + EXPECT_EQ(localizer->random_particles_to_inject(), 10u); +} + +} // namespace