completely shutdown dyn_params_handler_ (s)

nav2_amcl/src/amcl_node.cpp

@@ -312,7 +312,8 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   pose_pub_->on_deactivate();
   particle_cloud_pub_->on_deactivate();
 
-  // reset dynamic parameter handler
+  // shutdown and reset dynamic parameter handler
+  remove_on_set_parameters_callback(dyn_params_handler_.get());
   dyn_params_handler_.reset();
 
   // destroy bond connection

nav2_controller/src/controller_server.cpp

@@ -294,6 +294,8 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   publishZeroVelocity();
   vel_publisher_->on_deactivate();
+
+  remove_on_set_parameters_callback(dyn_params_handler_.get());
   dyn_params_handler_.reset();
 
   // destroy bond connection

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -78,6 +78,10 @@ InflationLayer::InflationLayer()
 
 InflationLayer::~InflationLayer()
 {
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
   dyn_params_handler_.reset();
   delete access_;
 }

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -62,6 +62,10 @@ namespace nav2_costmap_2d
 
 ObstacleLayer::~ObstacleLayer()
 {
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
   dyn_params_handler_.reset();
   for (auto & notifier : observation_notifiers_) {
     notifier.reset();

nav2_costmap_2d/plugins/static_layer.cpp

@@ -112,6 +112,10 @@ StaticLayer::activate()
 void
 StaticLayer::deactivate()
 {
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
   dyn_params_handler_.reset();
 }
 

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -116,6 +116,10 @@ void VoxelLayer::onInitialize()
 
 VoxelLayer::~VoxelLayer()
 {
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
   dyn_params_handler_.reset();
 }
 

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -329,6 +329,7 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Deactivating");
 
+  remove_on_set_parameters_callback(dyn_params_handler.get());
   dyn_params_handler.reset();
 
   stop();

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -131,6 +131,7 @@ DockingServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   navigator_->deactivate();
   vel_publisher_->on_deactivate();
 
+  remove_on_set_parameters_callback(dyn_params_handler_.get());
   dyn_params_handler_.reset();
   tf2_listener_.reset();
 

nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp

@@ -150,6 +150,7 @@ class KinematicsHandler
   using Ptr = std::shared_ptr<KinematicsHandler>;
 
 protected:
+  nav2_util::LifecycleNode::WeakPtr node_;
   std::atomic<KinematicParameters *> kinematics_;
 
   // Dynamic parameters handler

nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp

@@ -56,13 +56,19 @@ KinematicsHandler::KinematicsHandler()
 
 KinematicsHandler::~KinematicsHandler()
 {
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
+  dyn_params_handler_.reset();
   delete kinematics_.load();
 }
 
 void KinematicsHandler::initialize(
   const nav2_util::LifecycleNode::SharedPtr & nh,
   const std::string & plugin_name)
 {
+  node_ = nh;
   plugin_name_ = plugin_name;
 
   declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_x", rclcpp::ParameterValue(0.0));

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -70,13 +70,15 @@ class ParameterHandler
   /**
    * @brief Destructor for nav2_graceful_controller::ParameterHandler
    */
-  ~ParameterHandler() = default;
+  ~ParameterHandler();
 
   std::mutex & getMutex() {return mutex_;}
 
   Parameters * getParams() {return &params_;}
 
 protected:
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message

nav2_graceful_controller/src/parameter_handler.cpp

@@ -31,6 +31,7 @@ ParameterHandler::ParameterHandler(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name,
   rclcpp::Logger & logger, const double costmap_size_x)
 {
+  node_ = node;
   plugin_name_ = plugin_name;
   logger_ = logger;
 
@@ -103,6 +104,15 @@ ParameterHandler::ParameterHandler(
     std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1));
 }
 
+ParameterHandler::~ParameterHandler()
+{
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
+  dyn_params_handler_.reset();
+}
+
 rcl_interfaces::msg::SetParametersResult
 ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {

nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp

@@ -57,6 +57,11 @@ class ParametersHandler
   explicit ParametersHandler(
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent);
 
+  /**
+    * @brief Destructor for mppi::ParametersHandler
+    */
+  ~ParametersHandler();
+
   /**
     * @brief Starts processing dynamic parameter changes
     */

nav2_mppi_controller/src/parameters_handler.cpp

@@ -26,6 +26,15 @@ ParametersHandler::ParametersHandler(
   logger_ = node->get_logger();
 }
 
+ParametersHandler::~ParametersHandler()
+{
+  auto node = node_.lock();
+  if (on_set_param_handler_ && node) {
+    node->remove_on_set_parameters_callback(on_set_param_handler_.get());
+  }
+  on_set_param_handler_.reset();
+}
+
 void ParametersHandler::start()
 {
   auto node = node_.lock();

nav2_navfn_planner/src/navfn_planner.cpp

@@ -115,6 +115,10 @@ NavfnPlanner::deactivate()
   RCLCPP_INFO(
     logger_, "Deactivating plugin %s of type NavfnPlanner",
     name_.c_str());
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
   dyn_params_handler_.reset();
 }
 

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -81,13 +81,14 @@ class ParameterHandler
   /**
    * @brief Destrructor for nav2_regulated_pure_pursuit_controller::ParameterHandler
    */
-  ~ParameterHandler() = default;
+  ~ParameterHandler();
 
   std::mutex & getMutex() {return mutex_;}
 
   Parameters * getParams() {return &params_;}
 
 protected:
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -32,6 +32,7 @@ ParameterHandler::ParameterHandler(
   std::string & plugin_name, rclcpp::Logger & logger,
   const double costmap_size_x)
 {
+  node_ = node;
   plugin_name_ = plugin_name;
   logger_ = logger;
 
@@ -194,6 +195,15 @@ ParameterHandler::ParameterHandler(
       this, std::placeholders::_1));
 }
 
+ParameterHandler::~ParameterHandler()
+{
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
+  dyn_params_handler_.reset();
+}
+
 rcl_interfaces::msg::SetParametersResult
 ParameterHandler::dynamicParametersCallback(
   std::vector<rclcpp::Parameter> parameters)

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

@@ -124,6 +124,9 @@ void RotationShimController::deactivate()
 
   primary_controller_->deactivate();
 
+  if (auto node = node_.lock()) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
   dyn_params_handler_.reset();
 }
 

nav2_smac_planner/src/a_star.cpp

@@ -71,7 +71,7 @@ void AStarAlgorithm<NodeT>::initialize(
   _max_on_approach_iterations = max_on_approach_iterations;
   _terminal_checking_interval = terminal_checking_interval;
   _max_planning_time = max_planning_time;
-  if(!_is_initialized) {
+  if (!_is_initialized) {
     NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
   }
   _is_initialized = true;

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -177,6 +177,11 @@ void SmacPlanner2D::deactivate()
   if (_costmap_downsampler) {
     _costmap_downsampler->on_deactivate();
   }
+  // shutdown dyn_param_handler
+  auto node = _node.lock();
+  if (_dyn_params_handler && node) {
+    node->remove_on_set_parameters_callback(_dyn_params_handler.get());
+  }
   _dyn_params_handler.reset();
 }
 

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -311,6 +311,11 @@ void SmacPlannerHybrid::deactivate()
   if (_costmap_downsampler) {
     _costmap_downsampler->on_deactivate();
   }
+  // shutdown dyn_param_handler
+  auto node = _node.lock();
+  if (_dyn_params_handler && node) {
+    node->remove_on_set_parameters_callback(_dyn_params_handler.get());
+  }
   _dyn_params_handler.reset();
 }
 

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -250,6 +250,11 @@ void SmacPlannerLattice::deactivate()
     _expansions_publisher->on_deactivate();
     _planned_footprints_publisher->on_deactivate();
   }
+  // shutdown dyn_param_handler
+  auto node = _node.lock();
+  if (_dyn_params_handler && node) {
+    node->remove_on_set_parameters_callback(_dyn_params_handler.get());
+  }
   _dyn_params_handler.reset();
 }
 

nav2_theta_star_planner/src/theta_star_planner.cpp

@@ -86,6 +86,11 @@ void ThetaStarPlanner::activate()
 void ThetaStarPlanner::deactivate()
 {
   RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str());
+  auto node = parent_node_.lock();
+  if (node && dyn_params_handler_) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
+  dyn_params_handler_.reset();
 }
 
 nav_msgs::msg::Path ThetaStarPlanner::createPlan(

nav2_velocity_smoother/src/velocity_smoother.cpp

@@ -178,6 +178,8 @@ VelocitySmoother::on_deactivate(const rclcpp_lifecycle::State &)
     timer_.reset();
   }
   smoothed_cmd_pub_->on_deactivate();
+
+  remove_on_set_parameters_callback(dyn_params_handler_.get());
   dyn_params_handler_.reset();
 
   // destroy bond connection

nav2_waypoint_follower/src/waypoint_follower.cpp

@@ -154,6 +154,7 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   xyz_action_server_->deactivate();
   gps_action_server_->deactivate();
+  remove_on_set_parameters_callback(dyn_params_handler_.get());
   dyn_params_handler_.reset();
   // destroy bond connection
   destroyBond();