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,7 +78,12 @@ 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();
+ node.reset();
  delete access_;
 }
 

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -62,7 +62,12 @@ 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();
+ node.reset();
  for (auto & notifier : observation_notifiers_) {
  notifier.reset();
  }

nav2_costmap_2d/plugins/static_layer.cpp

@@ -112,7 +112,12 @@ 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();
+ node.reset();
 }
 
 void

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -116,7 +116,12 @@ 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();
+ node.reset();
 }
 
 void VoxelLayer::matchSize()

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::SharedPtr node_;
  std::atomic<KinematicParameters *> kinematics_;
 
  // Dynamic parameters handler

nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp

@@ -56,13 +56,16 @@ KinematicsHandler::KinematicsHandler()
 
 KinematicsHandler::~KinematicsHandler()
 {
+ 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::SharedPtr 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,12 @@ ParameterHandler::ParameterHandler(
  std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1));
 }
 
+ParameterHandler::~ParameterHandler()
+{
+ 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,16 @@ 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();
+ node.reset();
+}
+
 void ParametersHandler::start()
 {
  auto node = node_.lock();

nav2_navfn_planner/src/navfn_planner.cpp

@@ -115,7 +115,12 @@ 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();
+ node.reset();
 }
 
 void

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::SharedPtr 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,12 @@ ParameterHandler::ParameterHandler(
  this, std::placeholders::_1));
 }
 
+ParameterHandler::~ParameterHandler()
+{
+ 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,7 +177,13 @@ 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();
+ node.reset();
 }
 
 void SmacPlanner2D::cleanup()

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -311,7 +311,13 @@ 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();
+ node.reset();
 }
 
 void SmacPlannerHybrid::cleanup()

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -250,7 +250,13 @@ 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();
+ node.reset();
 }
 
 void SmacPlannerLattice::cleanup()

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();