diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 763a601ed8..8801d98d6b 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -34,6 +34,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_amcl/motion_model/motion_model.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" +#include "nav2_util/parameter_events_subscriber.hpp" #include "nav_msgs/srv/set_map.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" @@ -104,6 +105,10 @@ class AmclNode : public nav2_util::LifecycleNode tf2::Transform latest_tf_; void waitForTransforms(); + // Parameter Event Subscriber + std::shared_ptr param_subscriber_; + void parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & event); + // Message filters void initMessageFilters(); std::unique_ptr> laser_scan_sub_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index e857cf648d..3c24cc5b11 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -194,14 +194,21 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Configuring"); initParameters(); + if (always_reset_initial_pose_) { + initial_pose_is_known_ = false; + } initTransforms(); - initMessageFilters(); initPubSub(); + initMessageFilters(); initServices(); initOdometry(); initParticleFilter(); initLaserScan(); + param_subscriber_ = std::make_shared(shared_from_this()); + param_subscriber_->set_event_callback( + std::bind(&AmclNode::parameterEventCallback, this, std::placeholders::_1)); + return nav2_util::CallbackReturn::SUCCESS; } @@ -343,6 +350,15 @@ AmclNode::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } +void +AmclNode::parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & /*event*/) +{ + initParameters(); + initMessageFilters(); + initOdometry(); + initParticleFilter(); +} + void AmclNode::checkLaserReceived() { @@ -1040,10 +1056,6 @@ 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 @@ -1163,9 +1175,6 @@ AmclNode::initTransforms() void AmclNode::initMessageFilters() { - laser_scan_sub_ = std::make_unique>( - rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data); - laser_scan_filter_ = std::make_unique>( *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_); @@ -1193,6 +1202,9 @@ AmclNode::initPubSub() std::bind(&AmclNode::mapReceived, this, std::placeholders::_1)); RCLCPP_INFO(get_logger(), "Subscribed to map topic."); + + laser_scan_sub_ = std::make_unique>( + rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data); } void @@ -1225,7 +1237,9 @@ AmclNode::initOdometry() init_cov_[1] = last_published_pose_.pose.covariance[7]; init_cov_[2] = last_published_pose_.pose.covariance[35]; } - + if (motion_model_) { + motion_model_.reset(); + } motion_model_ = std::unique_ptr(nav2_amcl::MotionModel::createMotionModel( robot_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_)); @@ -1235,6 +1249,11 @@ AmclNode::initOdometry() void AmclNode::initParticleFilter() { + if (pf_ != nullptr) { + pf_free(pf_); + pf_ = nullptr; + } + // Create the particle filter pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,