diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 70df98bfc04e013560d544c0100d7e184ef31680..2b32cecd364915bcf987d2d52a7bbb1f0a6471de 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -231,10 +231,6 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - if (always_reset_initial_pose_) { - initial_pose_is_known_ = false; - } - // Lifecycle publishers must be explicitly activated pose_pub_->on_activate(); particlecloud_pub_->on_activate(); @@ -1044,6 +1040,10 @@ AmclNode::initParameters() " this isn't allowed so max_particles will be set to min_particles."); max_particles_ = min_particles_; } + + if (always_reset_initial_pose_) { + initial_pose_is_known_ = false; + } } void