Fix std::normal_distribution when stdev is zero

[fmrico]

Dear EasyNavers,

I found this execution error in Ubuntu 26.04:

[system_main-1] [WARN] [1784822583.116930278] [localizer_node]: [costmap] RT cycle time exceeded target by more than 1.5x (37.950 s > 0.020 s)
[system_main-1] /usr/include/c++/15/bits/random.h:2138: std::normal_distribution<_RealType>::param_type::param_type(_RealType, _RealType) [with _RealType = double]: Assertion '_M_stddev > _RealType(0)' failed.
[system_main-1] Magick: abort due to signal 6 (SIGABRT) "Abort"...

This PR fixes this bug by using a minumun of 1e-12 for stdev.

I hope it helps!!!

[Copilot]

Pull request overview

This PR prevents runtime aborts from std::normal_distribution when a computed standard deviation becomes zero by clamping distribution standard deviations to a small positive minimum across both AMCL localizers.

Changes:

• Clamp initialization noise (std_dev_xy, std_dev_yaw) to a minimum value when constructing normal distributions.
• Clamp motion-model noise in predict() (translation and yaw) to avoid zero-stdev distributions when there is no motion.
• Clamp additional distributions used during initialization/reseeding (e.g., yaw noise and index_dist) to avoid zero-stdev assertions.

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 2 comments.

File	Description
localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp	Adds min-stdev clamping for several normal distributions used in init, pose init callback, prediction, and reseeding.
localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp	Mirrors the same min-stdev clamping changes in the costmap-based AMCL implementation.

Comments suppressed due to low confidence (2)

localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp:643

• reseed() can be called with particles_.size() == 1, which makes N_top == 0. That leads to invalid covariance math (division by zero) and an infinite loop when selecting selected_idx because there is no valid index in [0, N_top). Add a guard for N_top == 0 and make the covariance-derived Cholesky terms robust to zero/degenerate covariance.

  double yaw_variance = computeYawVariance(particles_, 0, N_top);
  double yaw_stddev = std::sqrt(yaw_variance);
  std::normal_distribution<double> yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_));
  std::normal_distribution<double> index_dist(0.0, std::max(0.05 * static_cast<double>(N), 1e-12));
  std::normal_distribution<double> standard_normal(0.0, 1.0);

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp:687

• reseed() can be called with particles_.size() == 1, which makes N_top == 0. That leads to invalid covariance math (division by zero) and an infinite loop when selecting selected_idx because there is no valid index in [0, N_top). Add a guard for N_top == 0 and make the covariance-derived Cholesky terms robust to zero/degenerate covariance.

  double yaw_variance = computeYawVariance(particles_, 0, N_top);
  double yaw_stddev = std::sqrt(yaw_variance);
  std::normal_distribution<double> yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_));
  std::normal_distribution<double> index_dist(0.0, std::max(0.05 * static_cast<double>(N), 1e-12));
  std::normal_distribution<double> standard_normal(0.0, 1.0);

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.