diff --git a/nav2_amcl/README.md b/nav2_amcl/README.md index 5e8d737a760..031eadaad48 100644 --- a/nav2_amcl/README.md +++ b/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). diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 07bc79f63ef..799d06c48a2 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/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 /* diff --git a/nav2_amcl/include/nav2_amcl/pf/pf.hpp b/nav2_amcl/include/nav2_amcl/pf/pf.hpp index 50aeca31a97..c073153bd16 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf.hpp +++ b/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 + #include "nav2_amcl/pf/pf_vector.hpp" #include "nav2_amcl/pf/pf_kdtree.hpp" diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 6931b509545..301cc5d375e 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -23,6 +23,8 @@ #include "nav2_amcl/amcl_node.hpp" #include +#include +#include #include #include #include @@ -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(random_seed_)); + } else { + srand48(static_cast(std::time(nullptr))); + } + pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 980b4f9d979..fb69e2cb921 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -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; diff --git a/nav2_amcl/test/test_dynamic_parameters.cpp b/nav2_amcl/test/test_dynamic_parameters.cpp index 6fb25e294d5..523c7b228aa 100644 --- a/nav2_amcl/test/test_dynamic_parameters.cpp +++ b/nav2_amcl/test/test_dynamic_parameters.cpp @@ -25,7 +25,11 @@ TEST(WPTest, test_dynamic_parameters) { - auto amcl = std::make_shared(); + rclcpp::NodeOptions options; + // Ensure we cover the deterministic-seed branch in initParticleFilter(). + options.parameter_overrides({{"random_seed", 42}}); + + auto amcl = std::make_shared(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); diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 056bfb0dfe9..a21450f08f3 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/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