Fix distrib navmap

localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp

@@ -506,7 +506,8 @@ class UkfWrapper
     std::vector<bool> & updateVector, Eigen::VectorXd & measurement,
     Eigen::MatrixXd & measurementCovariance);
 
-  std::shared_ptr<easynav::LocalizerNode> parent_node_{nullptr};
+  // Not owned: the node owns the localizer (a shared_ptr here would leak the node).
+  easynav::LocalizerNode * parent_node_{nullptr};
 
   //! @brief Whether or not we print diagnostic messages to the /diagnostics
   //! topic

localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp

@@ -83,6 +83,33 @@ namespace robot_localization
 {
 using namespace std::chrono_literals;
 
+namespace
+{
+
+// The UKF averages the sigma points' angles circularly: past a variance of 2 rad^2 the mean flips
+// by pi and the filter diverges to NaN. Unobserved angles (a velocities-only filter) are capped.
+constexpr double kMaxAngleVariance = 1.0;
+
+void cap_angle_variances(Ukf & filter)
+{
+  Eigen::MatrixXd cov = filter.getEstimateErrorCovariance();
+  bool capped = false;
+  for (const int i : {StateMemberRoll, StateMemberPitch, StateMemberYaw}) {
+    if (cov(i, i) > kMaxAngleVariance) {
+      // Scaling row and column keeps the covariance positive semidefinite.
+      const double s = std::sqrt(kMaxAngleVariance / cov(i, i));
+      cov.row(i) *= s;
+      cov.col(i) *= s;
+      capped = true;
+    }
+  }
+  if (capped) {
+    filter.setEstimateErrorCovariance(cov);
+  }
+}
+
+}  // namespace
+
 UkfWrapper::UkfWrapper(
   std::shared_ptr<easynav::LocalizerNode> parent_node,
   const std::string & tf_prefix,
@@ -118,7 +145,7 @@ UkfWrapper::UkfWrapper(
   tf_time_offset_(0ns),
   local_filter_(local_filter)
 {
-  parent_node_ = parent_node;
+  parent_node_ = parent_node.get();
   tf_prefix_ = tf_prefix;
   plugin_name_ = plugin_name;
 
@@ -738,6 +765,7 @@ void UkfWrapper::integrateMeasurements(const rclcpp::Time & current_time)
 
       // This will call predict and, if necessary, correct
       filter_.processMeasurement(*(measurement.get()));
+      cap_angle_variances(filter_);
 
       // Store old states and measurements if we're smoothing
       if (smooth_lagged_data_) {
@@ -782,6 +810,7 @@ void UkfWrapper::integrateMeasurements(const rclcpp::Time & current_time)
 
     filter_.validateDelta(last_update_delta);
     filter_.predict(current_time, last_update_delta);
+    cap_angle_variances(filter_);
 
     // Update the last measurement time and last update time
     filter_.setLastMeasurementTime(

localizers/easynav_fusion_localizer/tests/CMakeLists.txt

@@ -19,3 +19,10 @@ target_link_libraries(fusion_localizer_reconfigure_tests
   rclcpp_lifecycle::rclcpp_lifecycle
   easynav_localizer::easynav_localizer
 )
+
+ament_add_gtest(fusion_localizer_local_filter_tests fusion_localizer_local_filter_tests.cpp)
+target_link_libraries(fusion_localizer_local_filter_tests
+  fusion_localizer
+  rclcpp::rclcpp
+  easynav_localizer::easynav_localizer
+)

localizers/easynav_fusion_localizer/tests/fusion_localizer_local_filter_tests.cpp

@@ -0,0 +1,273 @@
+// Copyright 2026 Intelligent Robotics Lab
+//
+// 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.
+
+/// \file
+/// \brief A local filter fusing only velocities (no absolute orientation) stays finite.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <optional>
+#include <utility>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "easynav_fusion_localizer/FusionLocalizer.hpp"
+#include "easynav_localizer/LocalizerNode.hpp"
+#include "easynav_common/types/NavState.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+
+namespace
+{
+
+class FriendFusionLocalizer : public easynav::FusionLocalizer
+{
+public:
+  using easynav::FusionLocalizer::update_rt;
+};
+
+bool finite(const nav_msgs::msg::Odometry & o)
+{
+  const auto & p = o.pose.pose;
+  return std::isfinite(p.position.x) && std::isfinite(p.position.y) &&
+         std::isfinite(p.orientation.z) && std::isfinite(p.orientation.w) &&
+         std::isfinite(o.twist.twist.linear.x) && std::isfinite(o.twist.twist.angular.z);
+}
+
+}  // namespace
+
+class FusionLocalFilterTest : public ::testing::Test
+{
+protected:
+  void SetUp() override {rclcpp::init(0, nullptr);}
+  void TearDown() override {rclcpp::shutdown();}
+
+  // Runs the localizer for \p seconds while the odometry reports (vx, wz); returns the last
+  // local estimate, if any.
+  std::optional<nav_msgs::msg::Odometry> run(
+    const std::vector<rclcpp::Parameter> & params, double vx, double wz, double seconds,
+    bool * all_finite)
+  {
+    rclcpp::NodeOptions options;
+    options.parameter_overrides(params);
+    auto node = std::make_shared<easynav::LocalizerNode>(options);
+    node->declare_parameter<std::vector<std::string>>(
+      "localizer_types", std::vector<std::string>{});
+    auto localizer = std::make_shared<FriendFusionLocalizer>();
+    localizer->initialize(node, "fusion");
+
+    auto pub_node = std::make_shared<rclcpp::Node>("local_filter_odom_pub");
+    auto pub = pub_node->create_publisher<nav_msgs::msg::Odometry>("test_odom", 10);
+    auto imu_pub = pub_node->create_publisher<sensor_msgs::msg::Imu>("test_imu", 10);
+    rclcpp::executors::SingleThreadedExecutor exec;
+    exec.add_node(node->get_node_base_interface());
+    // The filter's subscriptions are in the RT callback group.
+    exec.add_callback_group(node->get_real_time_cbg(), node->get_node_base_interface());
+    exec.add_node(pub_node->get_node_base_interface());
+
+    easynav::NavState nav_state;
+    std::optional<nav_msgs::msg::Odometry> last;
+    *all_finite = true;
+    const auto end = std::chrono::steady_clock::now() + std::chrono::duration<double>(seconds);
+    while (std::chrono::steady_clock::now() < end) {
+      nav_msgs::msg::Odometry odom;
+      odom.header.stamp = pub_node->now();
+      odom.header.frame_id = "odom";
+      odom.child_frame_id = "base_footprint";
+      odom.pose.pose.orientation.w = 1.0;
+      odom.twist.twist.linear.x = vx;
+      odom.twist.twist.angular.z = wz;
+      for (int i = 0; i < 6; ++i) {
+        odom.pose.covariance[i * 7] = 0.01;
+        odom.twist.covariance[i * 7] = 0.001;
+      }
+      pub->publish(odom);
+      sensor_msgs::msg::Imu imu;
+      imu.header.stamp = odom.header.stamp;
+      imu.header.frame_id = "base_footprint";
+      imu.orientation.w = 1.0;
+      imu.angular_velocity.z = wz;
+      imu.linear_acceleration.z = 9.81;
+      for (int i = 0; i < 3; ++i) {
+        imu.orientation_covariance[i * 4] = 0.01;
+        imu.angular_velocity_covariance[i * 4] = 0.0001;
+        imu.linear_acceleration_covariance[i * 4] = 0.01;
+      }
+      imu_pub->publish(imu);
+      exec.spin_some();
+      localizer->update_rt(nav_state);
+      if (nav_state.has("robot_pose_local")) {
+        last = nav_state.get<nav_msgs::msg::Odometry>("robot_pose_local");
+        *all_finite = *all_finite && finite(*last);
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    }
+    return last;
+  }
+
+  static std::vector<rclcpp::Parameter> velocities_only()
+  {
+    return {
+      {"fusion.local_filter.frequency", 30.0},
+      {"fusion.local_filter.two_d_mode", true},
+      {"fusion.local_filter.publish_tf", false},
+      {"fusion.local_filter.map_frame", "map"},
+      {"fusion.local_filter.odom_frame", "odom"},
+      {"fusion.local_filter.base_link_frame", "base_footprint"},
+      {"fusion.local_filter.world_frame", "odom"},
+      {"fusion.local_filter.odom0", "test_odom"},
+      {"fusion.local_filter.odom0_config", std::vector<bool>{
+          false, false, false, false, false, false,
+          true, true, false, false, false, true,
+          false, false, false}},
+    };
+  }
+
+  // Plus the IMU's yaw rate (as in the Summit playground).
+  static std::vector<rclcpp::Parameter> with_imu_yaw_rate()
+  {
+    auto params = velocities_only();
+    params.push_back({"fusion.local_filter.imu0", "test_imu"});
+    params.push_back({"fusion.local_filter.imu0_config", std::vector<bool>{
+          false, false, false, false, false, false,
+          false, false, false, false, false, true,
+          false, false, false}});
+    params.push_back({"fusion.local_filter.imu0_remove_gravitational_acceleration", true});
+    return params;
+  }
+};
+
+TEST_F(FusionLocalFilterTest, WithTheImuYawRateItStaysFinite)
+{
+  for (const auto & [vx, wz] : std::vector<std::pair<double, double>>{{0.0, 0.0}, {0.5, 0.3}}) {
+    bool all_finite = false;
+    const auto last = run(with_imu_yaw_rate(), vx, wz, 2.0, &all_finite);
+    ASSERT_TRUE(last.has_value());
+    EXPECT_TRUE(all_finite) << vx << ", " << wz;
+  }
+}
+
+TEST_F(FusionLocalFilterTest, BothFiltersStayFinite)
+{
+  auto params = with_imu_yaw_rate();
+  for (const auto & p : std::vector<rclcpp::Parameter>{
+    {"fusion.global_filter.frequency", 30.0},
+    {"fusion.global_filter.two_d_mode", true},
+    {"fusion.global_filter.publish_tf", false},
+    {"fusion.global_filter.map_frame", "map"},
+    {"fusion.global_filter.odom_frame", "odom"},
+    {"fusion.global_filter.base_link_frame", "base_footprint"},
+    {"fusion.global_filter.world_frame", "map"},
+    {"fusion.global_filter.odom0", "test_odom"},
+    {"fusion.global_filter.odom0_config", std::vector<bool>{
+        false, false, false, false, false, false,
+        true, true, false, false, false, true,
+        false, false, false}},
+    {"fusion.global_filter.imu0", "test_imu"},
+    {"fusion.global_filter.imu0_config", std::vector<bool>{
+        false, false, false, false, false, true,
+        false, false, false, false, false, true,
+        false, false, false}}})
+  {
+    params.push_back(p);
+  }
+  bool all_finite = false;
+  const auto last = run(params, 0.5, 0.3, 2.0, &all_finite);
+  ASSERT_TRUE(last.has_value());
+  EXPECT_TRUE(all_finite);
+}
+
+// Nothing observes the yaw, so its variance grows (0.06 rad^2/s by default) past 2 rad^2,
+// where the UKF's circular mean of the sigma points flips: here in ~1 s instead of ~33 s.
+TEST_F(FusionLocalFilterTest, AnUnobservedYawWithAGrowingVarianceStaysFinite)
+{
+  auto params = with_imu_yaw_rate();
+  const std::vector<double> noise{
+    0.05, 0.05, 0.06, 0.03, 0.03, 3.0, 0.025, 0.025, 0.04, 0.01, 0.01, 0.02, 0.01, 0.01, 0.015};
+  std::vector<double> process_noise(15 * 15, 0.0);
+  for (int i = 0; i < 15; ++i) {
+    process_noise[i * 16] = noise[i];
+  }
+  params.push_back({"fusion.local_filter.process_noise_covariance", process_noise});
+  for (const auto & [vx, wz] : std::vector<std::pair<double, double>>{{0.0, 0.0}, {0.5, 0.3}}) {
+    bool all_finite = false;
+    const auto last = run(params, vx, wz, 3.0, &all_finite);
+    ASSERT_TRUE(last.has_value());
+    EXPECT_TRUE(all_finite) << vx << ", " << wz;
+    // And the yaw did not jump by pi: it follows the turn rate.
+    const double yaw = 2.0 * std::atan2(last->pose.pose.orientation.z,
+      last->pose.pose.orientation.w);
+    EXPECT_LT(std::abs(std::remainder(yaw - wz * 3.0, 2.0 * M_PI)), 0.6) << vx << ", " << wz;
+  }
+}
+
+TEST_F(FusionLocalFilterTest, VelocitiesOnlyStayFiniteWhenStill)
+{
+  bool all_finite = false;
+  const auto last = run(velocities_only(), 0.0, 0.0, 2.0, &all_finite);
+  ASSERT_TRUE(last.has_value());
+  EXPECT_TRUE(all_finite);
+}
+
+TEST_F(FusionLocalFilterTest, VelocitiesOnlyIntegrateTheMotion)
+{
+  bool all_finite = false;
+  const auto last = run(velocities_only(), 0.5, 0.0, 2.0, &all_finite);
+  ASSERT_TRUE(last.has_value());
+  EXPECT_TRUE(all_finite);
+  EXPECT_GT(last->pose.pose.position.x, 0.4);   // about 1 m in 2 s
+  EXPECT_NEAR(last->twist.twist.linear.x, 0.5, 0.1);
+}
+
+TEST_F(FusionLocalFilterTest, VelocitiesOnlyStayFiniteWhenTurning)
+{
+  bool all_finite = false;
+  const auto last = run(velocities_only(), 0.3, 0.5, 2.0, &all_finite);
+  ASSERT_TRUE(last.has_value());
+  EXPECT_TRUE(all_finite);
+  EXPECT_NEAR(last->twist.twist.angular.z, 0.5, 0.15);
+}
+
+// The filters must not keep the node alive: the node owns the localizer, so a strong reference
+// back would leak it, and its DDS threads would still run at exit (segfault on shutdown).
+TEST_F(FusionLocalFilterTest, TheFiltersDoNotKeepTheNodeAlive)
+{
+  auto params = with_imu_yaw_rate();
+  params.push_back({"fusion.global_filter.frequency", 30.0});
+  params.push_back({"fusion.global_filter.odom0", "test_odom"});
+  params.push_back({"fusion.global_filter.odom0_config", std::vector<bool>{
+        false, false, false, false, false, false,
+        true, true, false, false, false, true,
+        false, false, false}});
+  rclcpp::NodeOptions options;
+  options.parameter_overrides(params);
+  auto node = std::make_shared<easynav::LocalizerNode>(options);
+  node->declare_parameter<std::vector<std::string>>(
+    "localizer_types", std::vector<std::string>{});
+  const auto before = node.use_count();
+  auto localizer = std::make_shared<FriendFusionLocalizer>();
+  localizer->initialize(node, "fusion");
+  EXPECT_EQ(node.use_count(), before);
+
+  std::weak_ptr<easynav::LocalizerNode> weak = node;
+  node.reset();
+  localizer.reset();
+  EXPECT_TRUE(weak.expired());
+}

localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp

@@ -729,16 +729,20 @@ void AMCLLocalizer::predict(NavState & nav_state)
   double r, p, yaw; tf2::Matrix3x3(delta.getRotation()).getRPY(r, p, yaw);
   double rot_len = std::abs(yaw);
 
-  std::normal_distribution<double> n_dx(0.0, std::abs(dx) * noise_translation_);
-  std::normal_distribution<double> n_dy(0.0, std::abs(dy) * noise_translation_);
-  std::normal_distribution<double> n_dz(0.0, std::abs(dz) * noise_translation_);
-  std::normal_distribution<double> n_yaw(0.0,
-    rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_);
+  // Zero-mean noise; a zero deviation (e.g. the robot still) is no noise: std::normal_distribution
+  // is undefined for it.
+  auto noise = [this](double stddev) {
+      return stddev > 0.0 ? std::normal_distribution<double>(0.0, stddev)(rng_) : 0.0;
+    };
+  const double sd_dx = std::abs(dx) * noise_translation_;
+  const double sd_dy = std::abs(dy) * noise_translation_;
+  const double sd_dz = std::abs(dz) * noise_translation_;
+  const double sd_yaw = rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_;
 
-  double noisy_y = yaw + n_yaw(rng_);
+  double noisy_y = yaw + noise(sd_yaw);
 
   for (auto & p : particles_) {
-    tf2::Vector3 noisy_t(dx + n_dx(rng_), dy + n_dy(rng_), dz + n_dz(rng_));
+    tf2::Vector3 noisy_t(dx + noise(sd_dx), dy + noise(sd_dy), dz + noise(sd_dz));
     tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_y);
     p.pose = p.pose * tf2::Transform(noisy_q, noisy_t);
 

localizers/easynav_navmap_localizer/tests/CMakeLists.txt

@@ -22,3 +22,9 @@ target_link_libraries(navmap_localizer_last_known_pose_tests
   ${PROJECT_NAME}
   rclcpp::rclcpp
 )
+
+ament_add_gtest(navmap_localizer_predict_tests navmap_localizer_predict_tests.cpp)
+target_link_libraries(navmap_localizer_predict_tests
+  ${PROJECT_NAME}
+  rclcpp::rclcpp
+)