diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 033cc8b..b07dfc2 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -242,9 +242,9 @@ AMCLLocalizer::on_initialize() RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); + std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_y(y_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_yaw(yaw_init, std::max(std_dev_yaw, 1e-12)); particles_.resize(num_particles); for (auto & p : particles_) { @@ -430,7 +430,7 @@ AMCLLocalizer::init_pose_callback( yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); std::normal_distribution standard_normal(0.0, 1.0); - std::normal_distribution yaw_noise(0.0, yaw_stddev); + std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, 1e-12)); const std::size_t N = particles_.size(); @@ -545,15 +545,17 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) double rot_len = std::abs(yaw); - for (auto & p : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); - - std::normal_distribution noise_yaw( - 0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); + std::normal_distribution noise_yaw( + 0.0, + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + for (auto & p : particles_) { tf2::Vector3 noisy_translation( dx + noise_dx(rng_), dy + noise_dy(rng_), @@ -680,7 +682,7 @@ AMCLLocalizer::reseed() double yaw_variance = computeYawVariance(particles_, 0, N_top); double yaw_stddev = std::sqrt(yaw_variance); std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_)); - std::normal_distribution index_dist(0.0, 0.05 * static_cast(N)); + std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); std::normal_distribution standard_normal(0.0, 1.0); for (std::size_t i = N_top; i < N; ++i) { diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 23729b9..43c5eda 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -239,9 +239,9 @@ AMCLLocalizer::on_initialize() RCLCPP_INFO(node->get_logger(), "at position (%lf, %lf, %lf) std_dev [%lf, %lf]", x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw); - std::normal_distribution noise_x(x_init, std_dev_xy); - std::normal_distribution noise_y(y_init, std_dev_xy); - std::normal_distribution noise_yaw(yaw_init, std_dev_yaw); + std::normal_distribution noise_x(x_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_y(y_init, std::max(std_dev_xy, 1e-12)); + std::normal_distribution noise_yaw(yaw_init, std::max(std_dev_yaw, 1e-12)); particles_.resize(num_particles); for (auto & p : particles_) { @@ -425,7 +425,7 @@ AMCLLocalizer::init_pose_callback( yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); std::normal_distribution standard_normal(0.0, 1.0); - std::normal_distribution yaw_noise(0.0, yaw_stddev); + std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, 1e-12)); const std::size_t N = particles_.size(); for (std::size_t i = 0; i < N; ++i) { @@ -502,15 +502,17 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) std::random_device rd; std::mt19937 gen(rd()); - for (auto & p : particles_) { - std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution noise_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution noise_dz(0.0, std::abs(dz) * noise_translation_); - - std::normal_distribution noise_yaw( - 0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + std::normal_distribution noise_dx( + 0.0, std::max(std::abs(dx) * noise_translation_, 1e-12)); + std::normal_distribution noise_dy( + 0.0, std::max(std::abs(dy) * noise_translation_, 1e-12)); + std::normal_distribution noise_dz( + 0.0, std::max(std::abs(dz) * noise_translation_, 1e-12)); + std::normal_distribution noise_yaw( + 0.0, + std::max(rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_, 1e-12)); + for (auto & p : particles_) { tf2::Vector3 noisy_translation( dx + noise_dx(gen), dy + noise_dy(gen), @@ -636,7 +638,7 @@ AMCLLocalizer::reseed() double yaw_variance = computeYawVariance(particles_, 0, N_top); double yaw_stddev = std::sqrt(yaw_variance); std::normal_distribution yaw_noise(0.0, std::max(yaw_stddev, min_noise_yaw_)); - std::normal_distribution index_dist(0.0, 0.05 * static_cast(N)); + std::normal_distribution index_dist(0.0, std::max(0.05 * static_cast(N), 1e-12)); std::normal_distribution standard_normal(0.0, 1.0); for (std::size_t i = N_top; i < N; ++i) {