diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index fd553bfb1..87c85397d 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -153,6 +154,11 @@ class AmclNode : public BaseAMCLNode { /// Likelihood field publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr likelihood_field_pub_; + /// Per-axis localization quality publishers (x, y, yaw). + rclcpp_lifecycle::LifecyclePublisher::SharedPtr quality_x_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr quality_y_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr quality_yaw_pub_; + /// Global relocalization service server. rclcpp::Service::SharedPtr global_localization_server_; /// No motion update service server. diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 2d4bee2d6..61fe8c2e5 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -201,6 +202,31 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", "" "increase resource usage and potentially degrade performance."; declare_parameter("debug", false, descriptor); } + + { + const auto defaults = beluga_ros::AmclParams{}; + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0.0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0.0; + + descriptor.description = + "Typical standard deviation of x in the output covariance matrix of a healthy filter, used for quality " + "estimation [m]."; + declare_parameter("expected_pose_x_stddev", rclcpp::ParameterValue(defaults.expected_pose_x_stddev), descriptor); + + descriptor.description = + "Typical standard deviation of y in the output covariance matrix of a healthy filter, used for quality " + "estimation [m]."; + declare_parameter("expected_pose_y_stddev", rclcpp::ParameterValue(defaults.expected_pose_y_stddev), descriptor); + + descriptor.description = + "Typical standard deviation of yaw in the output covariance matrix of a healthy filter, used for quality " + "estimation [rad]."; + declare_parameter( + "expected_pose_yaw_stddev", rclcpp::ParameterValue(defaults.expected_pose_yaw_stddev), descriptor); + } } AmclNode::~AmclNode() { @@ -215,6 +241,13 @@ void AmclNode::do_activate(const rclcpp_lifecycle::State&) { likelihood_field_pub_->on_activate(); } + quality_x_pub_ = create_publisher("localization_quality_x", rclcpp::SystemDefaultsQoS()); + quality_x_pub_->on_activate(); + quality_y_pub_ = create_publisher("localization_quality_y", rclcpp::SystemDefaultsQoS()); + quality_y_pub_->on_activate(); + quality_yaw_pub_ = create_publisher("localization_quality_yaw", rclcpp::SystemDefaultsQoS()); + quality_yaw_pub_->on_activate(); + { map_sub_ = create_subscription( get_parameter("map_topic").as_string(), rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -311,6 +344,15 @@ void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) { if (likelihood_field_pub_) { likelihood_field_pub_->on_deactivate(); } + if (quality_x_pub_) { + quality_x_pub_->on_deactivate(); + } + if (quality_y_pub_) { + quality_y_pub_->on_deactivate(); + } + if (quality_yaw_pub_) { + quality_yaw_pub_->on_deactivate(); + } } void AmclNode::do_cleanup(const rclcpp_lifecycle::State&) { @@ -318,6 +360,9 @@ void AmclNode::do_cleanup(const rclcpp_lifecycle::State&) { particle_filter_.reset(); enable_tf_broadcast_ = false; likelihood_field_pub_.reset(); + quality_x_pub_.reset(); + quality_y_pub_.reset(); + quality_yaw_pub_.reset(); } auto AmclNode::get_initial_estimate() const -> std::optional> { @@ -423,6 +468,9 @@ auto AmclNode::make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr map) params.spatial_resolution_x = get_parameter("spatial_resolution_x").as_double(); params.spatial_resolution_y = get_parameter("spatial_resolution_y").as_double(); params.spatial_resolution_theta = get_parameter("spatial_resolution_theta").as_double(); + params.expected_pose_x_stddev = get_parameter("expected_pose_x_stddev").as_double(); + params.expected_pose_y_stddev = get_parameter("expected_pose_y_stddev").as_double(); + params.expected_pose_yaw_stddev = get_parameter("expected_pose_yaw_stddev").as_double(); return std::make_unique( beluga_ros::OccupancyGrid{map}, // @@ -644,6 +692,15 @@ void AmclNode::sensor_callback(const std::shared_ptr& sensor_msg tf2::toMsg(base_pose_in_map, message.pose.pose); tf2::covarianceEigenToRowMajor(base_pose_covariance, message.pose.covariance); pose_pub_->publish(message); + + const auto quality = particle_filter_->quality(); + auto quality_msg = std_msgs::msg::Float64{}; + quality_msg.data = quality[0]; + quality_x_pub_->publish(quality_msg); + quality_msg.data = quality[1]; + quality_y_pub_->publish(quality_msg); + quality_msg.data = quality[2]; + quality_yaw_pub_->publish(quality_msg); } } diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index 46c612cb6..6b5093655 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -15,9 +15,11 @@ #ifndef BELUGA_ROS_AMCL_HPP #define BELUGA_ROS_AMCL_HPP +#include #include #include #include +#include #include #include @@ -95,6 +97,18 @@ struct AmclParams { /// \brief Spatial resolution around the z-axis to create buckets for KLD resampling. double spatial_resolution_theta = 10 * Sophus::Constants::pi() / 180; + + /// \brief Typical standard deviation of x in the output covariance matrix of a healthy filter, used for quality + /// estimation [m]. + double expected_pose_x_stddev = 0.1; + + /// \brief Typical standard deviation of y in the output covariance matrix of a healthy filter, used for quality + /// estimation [m]. + double expected_pose_y_stddev = 0.1; + + /// \brief Typical standard deviation of yaw in the output covariance matrix of a healthy filter, used for quality + /// estimation [rad]. + double expected_pose_yaw_stddev = 0.05; }; /// Implementation of the 2D Adaptive Monte Carlo Localization (AMCL) algorithm. @@ -262,6 +276,15 @@ class Amcl { /// Force a manual update of the particles on the next iteration of the filter. void force_update() { force_update_ = true; } + /// Returns the per-axis localization quality scores from the last filter update. + /** + * Each element is in [0, 1]: index 0 = x, 1 = y, 2 = yaw. + * A value close to 1 means the filter covariance for that axis is within the expected range; + * values approaching 0 indicate the filter may be diverging on that axis. + * Returns {1.0, 1.0, 1.0} before the first successful update. + */ + [[nodiscard]] std::array quality() const { return last_quality_; } + private: beluga::TupleVector particles_; @@ -279,6 +302,9 @@ class Amcl { beluga::RollingWindow control_action_window_; bool force_update_{true}; + std::array last_quality_{1.0, 1.0, 1.0}; + + std::array compute_quality(const Sophus::Matrix3d& actual_covariance); }; } // namespace beluga_ros diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index c38360e64..85e9cc84a 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -22,6 +23,8 @@ #include #include #include +#include +#include namespace beluga_ros { @@ -122,7 +125,25 @@ auto Amcl::update( } force_update_ = false; - return beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_)); + auto estimate = beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_)); + last_quality_ = compute_quality(estimate.second); + return estimate; +} + +std::array Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { + const std::array expected_variance = { + params_.expected_pose_x_stddev * params_.expected_pose_x_stddev, + params_.expected_pose_y_stddev * params_.expected_pose_y_stddev, + params_.expected_pose_yaw_stddev * params_.expected_pose_yaw_stddev, + }; + std::array quality{1.0, 1.0, 1.0}; + for (int i = 0; i < 3; ++i) { + const double actual = actual_covariance.coeff(i, i); + if (actual > std::numeric_limits::epsilon()) { + quality[i] = std::clamp(expected_variance[i] / actual, 0.0, 1.0); + } + } + return quality; } } // namespace beluga_ros