completely shutdown dyn_params_handler_ (s) (#4521)

GoesM · GoesM_server · web-flow · commit b19cb4d829a3 · 2024-07-18T21:32:16.000-07:00
* completely shutdown dny_params_handler_ in nav2_amcl

Signed-off-by: GoesM_server &lt;GoesM@buaa.edu.cn&gt;

* completely shutdown dyn_param_handler_ in controller_server

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* compeletly shutdown dyn_params_handler in nav2_costmap_2d

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* compeletly shutdown dyn_param_handler in nav2_docking

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* completely shutdown dyn_params_handler in nav2_velocity_smoother

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* compeletly shutdown dyn_param_handler in waypoint_follower

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* typo fixed

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* graceful-controller &amp; dwb_controller

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* mppi-controller

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* navfn_planner &amp; regulated_..controller

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* rotation_..controller &amp; samc_planners

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* A*planner

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* code style

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* 1

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* fixed

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* fix the usage of weak_ptr

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* code-style

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* weak_ptr released

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* code style

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* code style

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* code style

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* code style update

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* back

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* rebase conflict resovled

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* rebase error fixed

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* fixed2

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* rebase fixed 3

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* 33

Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;

* shared_ptr into weak_ptr

Signed-off-by: GoesM &lt;goesm@buaa.edu.cn&gt;

* remove adundant node.resest()

Signed-off-by: GoesM &lt;goesm@buaa.edu.cn&gt;

---------

Signed-off-by: GoesM_server &lt;GoesM@buaa.edu.cn&gt;
Signed-off-by: goes &lt;GoesM@buaa.edu.cn&gt;
Signed-off-by: GoesM &lt;goesm@buaa.edu.cn&gt;
Co-authored-by: GoesM_server &lt;GoesM@buaa.edu.cn&gt;
diff --git a/nav2_amcl/src/amcl_node.cpp b/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
diff --git a/nav2_controller/src/controller_server.cpp b/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
diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/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_;
 }
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/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();
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/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();
 }
 
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/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();
 }
 
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/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();
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/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();
 
diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/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
diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/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));
diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/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
diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/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)
 {
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp b/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
     */
diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/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();
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/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();
 }
 
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/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
diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/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)
diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/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();
 }
 
diff --git a/nav2_smac_planner/src/a_star.cpp b/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;
diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/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();
 }
 
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/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();
 }
 
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/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();
 }
 
diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/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(
diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/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
diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/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();

Original file line number	Diff line number	Diff line change
`@@ -78,6 +78,10 @@ InflationLayer::InflationLayer()`
`78`	`78`
`79`	`79`	`InflationLayer::~InflationLayer()`
`80`	`80`	`{`
	`81`	`+ auto node = node_.lock();`
	`82`	`+ if (dyn_params_handler_ && node) {`
	`83`	`+ node->remove_on_set_parameters_callback(dyn_params_handler_.get());`
	`84`	`+ }`
`81`	`85`	`dyn_params_handler_.reset();`
`82`	`86`	`delete access_;`
`83`	`87`	`}`
Original file line number	Diff line number	Diff line change
`@@ -112,6 +112,10 @@ StaticLayer::activate()`
`112`	`112`	`void`
`113`	`113`	`StaticLayer::deactivate()`
`114`	`114`	`{`
	`115`	`+ auto node = node_.lock();`
	`116`	`+ if (dyn_params_handler_ && node) {`
	`117`	`+ node->remove_on_set_parameters_callback(dyn_params_handler_.get());`
	`118`	`+ }`
`115`	`119`	`dyn_params_handler_.reset();`
`116`	`120`	`}`
`117`	`121`
Original file line number	Diff line number	Diff line change
`@@ -116,6 +116,10 @@ void VoxelLayer::onInitialize()`
`116`	`116`
`117`	`117`	`VoxelLayer::~VoxelLayer()`
`118`	`118`	`{`
	`119`	`+ auto node = node_.lock();`
	`120`	`+ if (dyn_params_handler_ && node) {`
	`121`	`+ node->remove_on_set_parameters_callback(dyn_params_handler_.get());`
	`122`	`+ }`
`119`	`123`	`dyn_params_handler_.reset();`
`120`	`124`	`}`
`121`	`125`
Original file line number	Diff line number	Diff line change
`@@ -329,6 +329,7 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /state/)`
`329`	`329`	`{`
`330`	`330`	`RCLCPP_INFO(get_logger(), "Deactivating");`
`331`	`331`
	`332`	`+ remove_on_set_parameters_callback(dyn_params_handler.get());`
`332`	`333`	`dyn_params_handler.reset();`
`333`	`334`
`334`	`335`	`stop();`
Original file line number	Diff line number	Diff line change
`@@ -56,13 +56,19 @@ KinematicsHandler::KinematicsHandler()`
`56`	`56`
`57`	`57`	`KinematicsHandler::~KinematicsHandler()`
`58`	`58`	`{`
	`59`	`+ auto node = node_.lock();`
	`60`	`+ if (dyn_params_handler_ && node) {`
	`61`	`+ node->remove_on_set_parameters_callback(dyn_params_handler_.get());`
	`62`	`+ }`
	`63`	`+ dyn_params_handler_.reset();`
`59`	`64`	`delete kinematics_.load();`
`60`	`65`	`}`
`61`	`66`
`62`	`67`	`void KinematicsHandler::initialize(`
`63`	`68`	`const nav2_util::LifecycleNode::SharedPtr & nh,`
`64`	`69`	`const std::string & plugin_name)`
`65`	`70`	`{`
	`71`	`+ node_ = nh;`
`66`	`72`	`plugin_name_ = plugin_name;`
`67`	`73`
`68`	`74`	`declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_x", rclcpp::ParameterValue(0.0));`