completely shutdown dyn_params_handler_ (s) (#4521)

* completely shutdown dny_params_handler_ in nav2_amcl Signed-off-by: GoesM_server <[email protected]> * completely shutdown dyn_param_handler_ in controller_server Signed-off-by: goes <[email protected]> * compeletly shutdown dyn_params_handler in nav2_costmap_2d Signed-off-by: goes <[email protected]> * compeletly shutdown dyn_param_handler in nav2_docking Signed-off-by: goes <[email protected]> * completely shutdown dyn_params_handler in nav2_velocity_smoother Signed-off-by: goes <[email protected]> * compeletly shutdown dyn_param_handler in waypoint_follower Signed-off-by: goes <[email protected]> * typo fixed Signed-off-by: goes <[email protected]> * graceful-controller & dwb_controller Signed-off-by: goes <[email protected]> * mppi-controller Signed-off-by: goes <[email protected]> * navfn_planner & regulated_..controller Signed-off-by: goes <[email protected]> * rotation_..controller & samc_planners Signed-off-by: goes <[email protected]> * A*planner Signed-off-by: goes <[email protected]> * code style Signed-off-by: goes <[email protected]> * 1 Signed-off-by: goes <[email protected]> * fixed Signed-off-by: goes <[email protected]> * fix the usage of weak_ptr Signed-off-by: goes <[email protected]> * code-style Signed-off-by: goes <[email protected]> * weak_ptr released Signed-off-by: goes <[email protected]> * code style Signed-off-by: goes <[email protected]> * code style Signed-off-by: goes <[email protected]> * code style Signed-off-by: goes <[email protected]> * code style update Signed-off-by: goes <[email protected]> * back Signed-off-by: goes <[email protected]> * rebase conflict resovled Signed-off-by: goes <[email protected]> * rebase error fixed Signed-off-by: goes <[email protected]> * fixed2 Signed-off-by: goes <[email protected]> * rebase fixed 3 Signed-off-by: goes <[email protected]> * 33 Signed-off-by: goes <[email protected]> * shared_ptr into weak_ptr Signed-off-by: GoesM <[email protected]> * remove adundant node.resest() Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM_server <[email protected]> Signed-off-by: goes <[email protected]> Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM_server <[email protected]>
ros-navigation · Jul 19, 2024 · b19cb4d · b19cb4d
1 parent 14e1e84
commit b19cb4d
Show file tree

Hide file tree

Showing 25 changed files with 99 additions and 4 deletions.
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/...e_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/...e_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 change
@@ Expand Up @@
     {
       RCLCPP_INFO(get_logger(), "Deactivating");
+      remove_on_set_parameters_callback(dyn_params_handler.get());
       dyn_params_handler.reset();
       stop();
@@ Expand Down @@