Nav2 AMCL: add random_seed param for deterministic replay; remove hidden time seeding in PF

nav2_amcl/README.md

@@ -2,3 +2,10 @@
 Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map using a 2D laser scanner. This is largely a refactored port from ROS 1 without any algorithmic changes.
 
 See the [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings.
+
+## Parameter note: `random_seed`
+
+AMCL supports a `random_seed` parameter to control the particle filter RNG seeding.
+
+- `random_seed >= 0`: seed the RNG with the provided value (repeatable runs).
+- `random_seed < 0` (default): seed the RNG from time (preserves historical behavior).

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -290,6 +290,7 @@ class AmclNode : public nav2::LifecycleNode
   bool pf_init_;
   pf_vector_t pf_odom_pose_;
   int resample_count_{0};
+  int random_seed_{-1};
 
   // Laser scan related
   /*

nav2_amcl/include/nav2_amcl/pf/pf.hpp

@@ -28,6 +28,8 @@
 #ifndef NAV2_AMCL__PF__PF_HPP_
 #define NAV2_AMCL__PF__PF_HPP_
 
+#include <stdint.h>
+
 #include "nav2_amcl/pf/pf_vector.hpp"
 #include "nav2_amcl/pf/pf_kdtree.hpp"
 

nav2_amcl/src/amcl_node.cpp

@@ -23,6 +23,8 @@
 #include "nav2_amcl/amcl_node.hpp"
 
 #include <algorithm>
+#include <ctime>
+#include <cstdint>
 #include <memory>
 #include <string>
 #include <utility>
@@ -956,6 +958,7 @@ AmclNode::initParameters()
   freespace_downsampling_ = this->declare_or_get_parameter("freespace_downsampling", false);
   allow_parameter_qos_overrides_ = this->declare_or_get_parameter(
     "allow_parameter_qos_overrides", true);
+  random_seed_ = this->declare_or_get_parameter("random_seed", -1);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
   transform_tolerance_ = tf2::durationFromSec(tmp_tol);
@@ -1463,6 +1466,17 @@ AmclNode::initParticleFilter()
   pf_ = pf_alloc(
     min_particles_, max_particles_, alpha_slow_, alpha_fast_,
     (pf_init_model_fn_t)AmclNode::uniformPoseGenerator);
+
+  // Seed RNG used by PF resampling and pose generation.
+  // Keep legacy behavior (time-based) unless user explicitly sets a seed.
+  if (random_seed_ >= 0) {
+    // `srand48` expects a platform `long` seed. We avoid using `long` in our code and accept
+    // truncation when seeding.
+    srand48(static_cast<int>(random_seed_));
+  } else {
+    srand48(static_cast<int>(std::time(nullptr)));
+  }
+
   pf_->pop_err = pf_err_;
   pf_->pop_z = pf_z_;
 

nav2_amcl/src/pf/pf.c

@@ -28,6 +28,7 @@
 #include <float.h>
 #include <assert.h>
 #include <math.h>
+#include <stdint.h>
 #include <stdlib.h>
 #include <time.h>
 
@@ -54,8 +55,6 @@ pf_t * pf_alloc(
   pf_sample_set_t * set;
   pf_sample_t * sample;
 
-  srand48(time(NULL));
-
   pf = calloc(1, sizeof(pf_t));
 
   pf->random_pose_fn = random_pose_fn;

nav2_amcl/test/test_dynamic_parameters.cpp

@@ -25,7 +25,11 @@
 
 TEST(WPTest, test_dynamic_parameters)
 {
-  auto amcl = std::make_shared<nav2_amcl::AmclNode>();
+  rclcpp::NodeOptions options;
+  // Ensure we cover the deterministic-seed branch in initParticleFilter().
+  options.parameter_overrides({{"random_seed", 42}});
+
+  auto amcl = std::make_shared<nav2_amcl::AmclNode>(options);
   amcl->configure();
   amcl->activate();
 
@@ -118,6 +122,7 @@ TEST(WPTest, test_dynamic_parameters)
   EXPECT_EQ(amcl->get_parameter("do_beamskip").as_bool(), false);
   EXPECT_EQ(amcl->get_parameter("set_initial_pose").as_bool(), false);
   EXPECT_EQ(amcl->get_parameter("first_map_only").as_bool(), false);
+  EXPECT_EQ(amcl->get_parameter("random_seed").as_int(), 42);
 
   results = rec_param->set_parameters_atomically({rclcpp::Parameter("alpha1", -1.0)});
   rclcpp::spin_until_future_complete(amcl->get_node_base_interface(), results);

nav2_bringup/params/nav2_params.yaml

@@ -24,6 +24,7 @@ amcl:
     pf_z: 0.99
     recovery_alpha_fast: 0.0
     recovery_alpha_slow: 0.0
+    random_seed: -1
     resample_interval: 1
     robot_model_type: "nav2_amcl::DifferentialMotionModel"
     save_pose_rate: 0.5