diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 1a10c7aa41..b5e512ba94 100644 --- 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 index 16c3db43a8..06a51f89ef 100644 --- 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 index 01b788a7cc..6ce187a0b2 100644 --- 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 index 057acb125d..6321305dd7 100644 --- 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 index 25a10e3bd8..67b7fbd5ec 100644 --- 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 index fc411d9f42..c4344d9f0e 100644 --- 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 index 6783c79492..5cba6b70cb 100644 --- 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 index d12d3f526b..430ec78883 100644 --- 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 index bfed0c2ec5..a87376e718 100644 --- 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; protected: + nav2_util::LifecycleNode::WeakPtr node_; std::atomic 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 index 2ca2b71f58..dd7d5ff1e6 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -56,6 +56,11 @@ 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(); } @@ -63,6 +68,7 @@ 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 index 74d56d48f6..7594524cf0 100644 --- 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 ¶ms_;} 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 index 61bfc7a9df..3d3389ec1d 100644 --- 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 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 index 1667fa6d79..fbb6760268 100644 --- 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 index fd284b60d4..c66d572561 100644 --- 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 index 9985280159..7c94751a76 100644 --- 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 index b033289622..66a5312343 100644 --- 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 ¶ms_;} 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 index 42876c248e..298ff2bf93 100644 --- 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 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 index dde73151fd..afb74c357e 100644 --- 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 index 3c22ce450f..aa95567123 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -71,7 +71,7 @@ void AStarAlgorithm::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 index 00cc07b464..728d9910fd 100644 --- 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 index 36201b26de..5ba10d7d9a 100644 --- 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 index 10605094e4..c5a9ecdff6 100644 --- 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 index 61064efbf4..24f2b81000 100644 --- 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 index 02fb390cc0..535b6b08f2 100644 --- 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 index 8dfd9b1461..02a4158d28 100644 --- 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();