From 870b7f34651d2bd50979f5602ec0c61659501d5e Mon Sep 17 00:00:00 2001 From: GoesM_server Date: Fri, 5 Jul 2024 08:12:41 +0000 Subject: [PATCH 01/30] completely shutdown dny_params_handler_ in nav2_amcl Signed-off-by: GoesM_server --- nav2_amcl/src/amcl_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 1a10c7aa417..b5e512ba946 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 From c46850cda20474f80f23dc83129d7d423862da15 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 00:12:47 +0800 Subject: [PATCH 02/30] completely shutdown dyn_param_handler_ in controller_server Signed-off-by: goes --- nav2_controller/src/controller_server.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 16c3db43a8a..c4c1218069c 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_param_handler_.get()); dyn_params_handler_.reset(); // destroy bond connection From acacabe4afda1ac2206412c1039802296609e801 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 00:31:57 +0800 Subject: [PATCH 03/30] compeletly shutdown dyn_params_handler in nav2_costmap_2d Signed-off-by: goes --- nav2_costmap_2d/plugins/inflation_layer.cpp | 1 + nav2_costmap_2d/plugins/obstacle_layer.cpp | 1 + nav2_costmap_2d/plugins/static_layer.cpp | 1 + nav2_costmap_2d/plugins/voxel_layer.cpp | 1 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 1 + 5 files changed, 5 insertions(+) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 01b788a7ccf..999d5d9481b 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,6 +78,7 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { + node_.lock()->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 057acb125d3..38a6258e309 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -62,6 +62,7 @@ namespace nav2_costmap_2d ObstacleLayer::~ObstacleLayer() { + node_.lock()->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 25a10e3bd8f..ef011eaf65c 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -112,6 +112,7 @@ StaticLayer::activate() void StaticLayer::deactivate() { + node_.lock()->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 fc411d9f42e..a99466e6339 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -116,6 +116,7 @@ void VoxelLayer::onInitialize() VoxelLayer::~VoxelLayer() { + node_.lock()->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 6783c794924..dd9559f2dfd 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_param_handler_.get()); dyn_params_handler.reset(); stop(); From 2900f7923ee7092aa16bc55f52f6957336bd0837 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 00:46:34 +0800 Subject: [PATCH 04/30] compeletly shutdown dyn_param_handler in nav2_docking Signed-off-by: goes --- nav2_docking/opennav_docking/src/docking_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index d12d3f526be..430ec78883b 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(); From 4a6cce9deb386b5a04ae25d73af17ddd4df0b38f Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:00:07 +0800 Subject: [PATCH 05/30] completely shutdown dyn_params_handler in nav2_velocity_smoother Signed-off-by: goes --- nav2_velocity_smoother/src/velocity_smoother.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 02fb390cc0c..535b6b08f21 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 From 782698a897afc91162642a490286c46364fcbc2f Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:17:58 +0800 Subject: [PATCH 06/30] compeletly shutdown dyn_param_handler in waypoint_follower Signed-off-by: goes --- nav2_waypoint_follower/src/waypoint_follower.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 8dfd9b1461e..02a4158d280 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(); From 134e747a8f93a2866febe16598bc7d858f5dcb75 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:24:21 +0800 Subject: [PATCH 07/30] typo fixed Signed-off-by: goes --- nav2_controller/src/controller_server.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c4c1218069c..06a51f89efa 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -295,7 +295,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) publishZeroVelocity(); vel_publisher_->on_deactivate(); - remove_on_set_parameters_callback(dyn_param_handler_.get()); + remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); // destroy bond connection diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index dd9559f2dfd..29acee64b11 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -329,7 +329,7 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - remove_on_set_parameters_callback(dyn_param_handler_.get()); + remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler.reset(); stop(); From 109da60c71e98be96087bcf1bdb6417c4e937463 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:32:51 +0800 Subject: [PATCH 08/30] graceful-controller & dwb_controller Signed-off-by: goes --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- .../include/dwb_plugins/kinematic_parameters.hpp | 1 + .../dwb_plugins/src/kinematic_parameters.cpp | 3 +++ .../include/nav2_graceful_controller/parameter_handler.hpp | 2 ++ nav2_graceful_controller/src/parameter_handler.cpp | 7 +++++++ 5 files changed, 14 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 29acee64b11..5cba6b70cb9 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -329,7 +329,7 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - remove_on_set_parameters_callback(dyn_params_handler_.get()); + remove_on_set_parameters_callback(dyn_params_handler.get()); dyn_params_handler.reset(); stop(); 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 bfed0c2ec54..63918bb3b94 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::SharedPtr 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 2ca2b71f582..719b7feacc0 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -56,6 +56,8 @@ KinematicsHandler::KinematicsHandler() KinematicsHandler::~KinematicsHandler() { + node_->remove_on_set_parameters_callback(dyn_params_handler_.get()); + dyn_params_handler_.reset(); delete kinematics_.load(); } @@ -63,6 +65,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 74d56d48f6c..bf4c90afad6 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -77,6 +77,8 @@ class ParameterHandler Parameters * getParams() {return ¶ms_;} protected: + rclcpp_lifecycle::LifecycleNode::SharedPtr 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 61bfc7a9df2..7eeef061ac5 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,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 parameters) { From c2f0548a031d6af864c9d2fa3804252f3327bb3b Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:37:41 +0800 Subject: [PATCH 09/30] mppi-controller Signed-off-by: goes --- .../nav2_mppi_controller/tools/parameters_handler.hpp | 5 +++++ nav2_mppi_controller/src/parameters_handler.cpp | 6 ++++++ 2 files changed, 11 insertions(+) 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 1667fa6d79d..fbb6760268f 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 fd284b60d47..c6fe7d85514 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -26,6 +26,12 @@ ParametersHandler::ParametersHandler( logger_ = node->get_logger(); } +ParametersHandler::~ParametersHandler() +{ + node_.lock()->remove_on_set_parameters_callback(on_set_param_handler_.get()); + on_set_param_handler_.reset(); +} + void ParametersHandler::start() { auto node = node_.lock(); From f01aba701d8b9056f3f3008e32af589b8a9e94db Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:40:32 +0800 Subject: [PATCH 10/30] navfn_planner & regulated_..controller Signed-off-by: goes --- nav2_navfn_planner/src/navfn_planner.cpp | 1 + .../parameter_handler.hpp | 1 + .../src/parameter_handler.cpp | 7 +++++++ 3 files changed, 9 insertions(+) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 9985280159e..b302552e7a1 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -115,6 +115,7 @@ NavfnPlanner::deactivate() RCLCPP_INFO( logger_, "Deactivating plugin %s of type NavfnPlanner", name_.c_str()); + node_.lock()->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 b033289622f..ed532edbe97 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 @@ -88,6 +88,7 @@ class ParameterHandler Parameters * getParams() {return ¶ms_;} protected: + rclcpp_lifecycle::LifecycleNode::SharedPtr 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 42876c248e7..5149844ebf1 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,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 parameters) From ee1bec5a4aa0e6d80911c6288117c12c59765c4f Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:43:00 +0800 Subject: [PATCH 11/30] rotation_..controller & samc_planners Signed-off-by: goes --- .../src/nav2_rotation_shim_controller.cpp | 1 + nav2_smac_planner/src/smac_planner_2d.cpp | 1 + nav2_smac_planner/src/smac_planner_hybrid.cpp | 1 + nav2_smac_planner/src/smac_planner_lattice.cpp | 1 + 4 files changed, 4 insertions(+) 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 dde73151fda..95654c3717f 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,7 @@ void RotationShimController::deactivate() primary_controller_->deactivate(); + node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_); dyn_params_handler_.reset(); } diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 00cc07b464b..ee51bb813cb 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -177,6 +177,7 @@ void SmacPlanner2D::deactivate() if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } + _node.lock()->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 36201b26de4..88ae63d4659 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -311,6 +311,7 @@ void SmacPlannerHybrid::deactivate() if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } + _node.lock()->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 10605094e45..212e071f242 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -250,6 +250,7 @@ void SmacPlannerLattice::deactivate() _expansions_publisher->on_deactivate(); _planned_footprints_publisher->on_deactivate(); } + _node.lock()->remove_on_set_parameters_callback(_dyn_params_handler.get()); _dyn_params_handler.reset(); } From e5266fbcd8868d13d036ad139bd98ed6f73b0aa9 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:44:32 +0800 Subject: [PATCH 12/30] A*planner Signed-off-by: goes --- nav2_theta_star_planner/src/theta_star_planner.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 61064efbf45..c10cda5348e 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -86,6 +86,8 @@ void ThetaStarPlanner::activate() void ThetaStarPlanner::deactivate() { RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); + parent_node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); + dyn_params_handler_.reset(); } nav_msgs::msg::Path ThetaStarPlanner::createPlan( From b2897db40acd7455fffcd8ac7d0e736a518e011e Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 01:47:02 +0800 Subject: [PATCH 13/30] code style Signed-off-by: goes --- .../include/nav2_graceful_controller/parameter_handler.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 bf4c90afad6..bb214178f69 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -78,7 +78,7 @@ class ParameterHandler protected: rclcpp_lifecycle::LifecycleNode::SharedPtr node_; - + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message From 0361ab6c0fc9a57d55b3f732b52d7de05319e507 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 02:29:39 +0800 Subject: [PATCH 14/30] 1 Signed-off-by: goes --- .../include/nav2_graceful_controller/parameter_handler.hpp | 2 +- .../parameter_handler.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 bb214178f69..cbc57848d68 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -70,7 +70,7 @@ class ParameterHandler /** * @brief Destructor for nav2_graceful_controller::ParameterHandler */ - ~ParameterHandler() = default; + ~ParameterHandler(); std::mutex & getMutex() {return mutex_;} 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 ed532edbe97..5d646c8460e 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,7 +81,7 @@ class ParameterHandler /** * @brief Destrructor for nav2_regulated_pure_pursuit_controller::ParameterHandler */ - ~ParameterHandler() = default; + ~ParameterHandler(); std::mutex & getMutex() {return mutex_;} From 3726cc36c784b51673493386930cad67f107e054 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 02:34:43 +0800 Subject: [PATCH 15/30] fixed Signed-off-by: goes --- .../src/nav2_rotation_shim_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 95654c3717f..0489a672557 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -124,7 +124,7 @@ void RotationShimController::deactivate() primary_controller_->deactivate(); - node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_); + node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); } From 71a17fccd125796417bd8b7a996721fe93012449 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 16:11:39 +0800 Subject: [PATCH 16/30] fix the usage of weak_ptr Signed-off-by: goes --- nav2_costmap_2d/plugins/inflation_layer.cpp | 3 ++- nav2_costmap_2d/plugins/obstacle_layer.cpp | 3 ++- nav2_costmap_2d/plugins/static_layer.cpp | 3 ++- nav2_costmap_2d/plugins/voxel_layer.cpp | 3 ++- nav2_mppi_controller/src/parameters_handler.cpp | 3 ++- nav2_navfn_planner/src/navfn_planner.cpp | 3 ++- .../src/nav2_rotation_shim_controller.cpp | 3 ++- nav2_smac_planner/src/smac_planner_2d.cpp | 3 ++- nav2_smac_planner/src/smac_planner_hybrid.cpp | 3 ++- nav2_smac_planner/src/smac_planner_lattice.cpp | 3 ++- nav2_theta_star_planner/src/theta_star_planner.cpp | 3 ++- 11 files changed, 22 insertions(+), 11 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 999d5d9481b..e91392de5bb 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,7 +78,8 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { - node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); + auto node = node_.lock(); + 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 38a6258e309..4e0001b66b5 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -62,7 +62,8 @@ namespace nav2_costmap_2d ObstacleLayer::~ObstacleLayer() { - node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); + auto node = node_.lock(); + 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 ef011eaf65c..c6d74dcd0f8 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -112,7 +112,8 @@ StaticLayer::activate() void StaticLayer::deactivate() { - node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); + auto node = node_.lock(); + 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 a99466e6339..139d524ba8e 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -116,7 +116,8 @@ void VoxelLayer::onInitialize() VoxelLayer::~VoxelLayer() { - node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); + auto node = node_.lock(); + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); } diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index c6fe7d85514..fd8680d0794 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -28,7 +28,8 @@ ParametersHandler::ParametersHandler( ParametersHandler::~ParametersHandler() { - node_.lock()->remove_on_set_parameters_callback(on_set_param_handler_.get()); + auto node = node_.lock(); + node->remove_on_set_parameters_callback(on_set_param_handler_.get()); on_set_param_handler_.reset(); } diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index b302552e7a1..db45cd0fe5d 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -115,7 +115,8 @@ NavfnPlanner::deactivate() RCLCPP_INFO( logger_, "Deactivating plugin %s of type NavfnPlanner", name_.c_str()); - node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); + auto node = node_.lock(); + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); } 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 0489a672557..fc4b17d0544 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -124,7 +124,8 @@ void RotationShimController::deactivate() primary_controller_->deactivate(); - node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); + 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/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index ee51bb813cb..bbdb1afe7c6 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -177,7 +177,8 @@ void SmacPlanner2D::deactivate() if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } - _node.lock()->remove_on_set_parameters_callback(_dyn_params_handler.get()); + 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/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 88ae63d4659..8dcd7204a5c 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -311,7 +311,8 @@ void SmacPlannerHybrid::deactivate() if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } - _node.lock()->remove_on_set_parameters_callback(_dyn_params_handler.get()); + 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/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 212e071f242..3e469874d20 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -250,7 +250,8 @@ void SmacPlannerLattice::deactivate() _expansions_publisher->on_deactivate(); _planned_footprints_publisher->on_deactivate(); } - _node.lock()->remove_on_set_parameters_callback(_dyn_params_handler.get()); + auto node=_node.lock(); + 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 c10cda5348e..1e4458877d8 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -86,7 +86,8 @@ void ThetaStarPlanner::activate() void ThetaStarPlanner::deactivate() { RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); - parent_node_.lock()->remove_on_set_parameters_callback(dyn_params_handler_.get()); + auto node = parent_node_.lock(); + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); dyn_params_handler_.reset(); } From 350016ee48fe8aeb99735e13175384c328729893 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 16:15:34 +0800 Subject: [PATCH 17/30] code-style Signed-off-by: goes --- nav2_smac_planner/src/smac_planner_hybrid.cpp | 2 +- nav2_smac_planner/src/smac_planner_lattice.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 8dcd7204a5c..a16458ddc2f 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -311,7 +311,7 @@ void SmacPlannerHybrid::deactivate() if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } - auto node=_node.lock(); + 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/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 3e469874d20..0b7dbd0361b 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -250,7 +250,7 @@ void SmacPlannerLattice::deactivate() _expansions_publisher->on_deactivate(); _planned_footprints_publisher->on_deactivate(); } - auto node=_node.lock(); + auto node = _node.lock(); node->remove_on_set_parameters_callback(_dyn_params_handler.get()); _dyn_params_handler.reset(); } From 3251a15fe6ad2dcd5f516ae33c673f0ab0d23f5c Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 16:59:21 +0800 Subject: [PATCH 18/30] weak_ptr released Signed-off-by: goes --- nav2_costmap_2d/plugins/inflation_layer.cpp | 6 ++++-- nav2_costmap_2d/plugins/obstacle_layer.cpp | 6 ++++-- nav2_costmap_2d/plugins/static_layer.cpp | 6 ++++-- nav2_costmap_2d/plugins/voxel_layer.cpp | 6 ++++-- nav2_mppi_controller/src/parameters_handler.cpp | 6 ++++-- nav2_navfn_planner/src/navfn_planner.cpp | 6 ++++-- .../src/nav2_rotation_shim_controller.cpp | 6 ++++-- nav2_smac_planner/src/smac_planner_2d.cpp | 7 +++++-- nav2_smac_planner/src/smac_planner_hybrid.cpp | 7 +++++-- nav2_smac_planner/src/smac_planner_lattice.cpp | 7 +++++-- nav2_theta_star_planner/src/theta_star_planner.cpp | 6 ++++-- 11 files changed, 47 insertions(+), 22 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index e91392de5bb..a72fc0b3dae 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,8 +78,10 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { - auto node = node_.lock(); - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + if(auto node = node_.lock()) + { + 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 4e0001b66b5..c87fa8b42a0 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -62,8 +62,10 @@ namespace nav2_costmap_2d ObstacleLayer::~ObstacleLayer() { - auto node = node_.lock(); - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + if(auto node = node_.lock()) + { + 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 c6d74dcd0f8..02a6fd7a2a8 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -112,8 +112,10 @@ StaticLayer::activate() void StaticLayer::deactivate() { - auto node = node_.lock(); - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + if(auto node = node_.lock()) + { + 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 139d524ba8e..1e3dc578f46 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -116,8 +116,10 @@ void VoxelLayer::onInitialize() VoxelLayer::~VoxelLayer() { - auto node = node_.lock(); - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + if(auto node = node_.lock()) + { + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + } dyn_params_handler_.reset(); } diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index fd8680d0794..3189d81d1ef 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -28,8 +28,10 @@ ParametersHandler::ParametersHandler( ParametersHandler::~ParametersHandler() { - auto node = node_.lock(); - node->remove_on_set_parameters_callback(on_set_param_handler_.get()); + if(auto node = node_.lock()) + { + node->remove_on_set_parameters_callback(on_set_param_handler_.get()); + } on_set_param_handler_.reset(); } diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index db45cd0fe5d..0baaea9bc3c 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -115,8 +115,10 @@ NavfnPlanner::deactivate() RCLCPP_INFO( logger_, "Deactivating plugin %s of type NavfnPlanner", name_.c_str()); - auto node = node_.lock(); - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + if(auto node = node_.lock()) + { + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + } dyn_params_handler_.reset(); } 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 fc4b17d0544..f1068cde00e 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -124,8 +124,10 @@ void RotationShimController::deactivate() primary_controller_->deactivate(); - auto node = node_.lock(); - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + 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/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index bbdb1afe7c6..207e157fd3e 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -177,8 +177,11 @@ void SmacPlanner2D::deactivate() if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } - auto node = _node.lock(); - node->remove_on_set_parameters_callback(_dyn_params_handler.get()); + // shutdown dyn_param_handler + 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/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index a16458ddc2f..8679e1b1963 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -311,8 +311,11 @@ void SmacPlannerHybrid::deactivate() if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } - auto node = _node.lock(); - node->remove_on_set_parameters_callback(_dyn_params_handler.get()); + // shutdown dyn_param_handler + 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/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 0b7dbd0361b..b040210bf4d 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -250,8 +250,11 @@ void SmacPlannerLattice::deactivate() _expansions_publisher->on_deactivate(); _planned_footprints_publisher->on_deactivate(); } - auto node = _node.lock(); - node->remove_on_set_parameters_callback(_dyn_params_handler.get()); + // shutdown dyn_param_handler + if(auto node = _node.lock()) + { + 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 1e4458877d8..156191659bf 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -86,8 +86,10 @@ 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(); - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + if(auto node = parent_node_.lock()) + { + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + } dyn_params_handler_.reset(); } From de825b767e63acfb4d872c96810505c46827d4a9 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 17:03:35 +0800 Subject: [PATCH 19/30] code style Signed-off-by: goes --- nav2_costmap_2d/plugins/inflation_layer.cpp | 3 +-- nav2_costmap_2d/plugins/obstacle_layer.cpp | 3 +-- nav2_costmap_2d/plugins/static_layer.cpp | 3 +-- nav2_costmap_2d/plugins/voxel_layer.cpp | 3 +-- nav2_mppi_controller/src/parameters_handler.cpp | 3 +-- nav2_navfn_planner/src/navfn_planner.cpp | 3 +-- .../src/nav2_rotation_shim_controller.cpp | 3 +-- 7 files changed, 7 insertions(+), 14 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index a72fc0b3dae..6b2e16d90bf 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,8 +78,7 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { - if(auto node = node_.lock()) - { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index c87fa8b42a0..ab40e214c43 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -62,8 +62,7 @@ namespace nav2_costmap_2d ObstacleLayer::~ObstacleLayer() { - if(auto node = node_.lock()) - { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 02a6fd7a2a8..681faa27de1 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -112,8 +112,7 @@ StaticLayer::activate() void StaticLayer::deactivate() { - if(auto node = node_.lock()) - { + if(auto node = node_.lock()) { 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 1e3dc578f46..9011c2c433a 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -116,8 +116,7 @@ void VoxelLayer::onInitialize() VoxelLayer::~VoxelLayer() { - if(auto node = node_.lock()) - { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index 3189d81d1ef..956300e073b 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -28,8 +28,7 @@ ParametersHandler::ParametersHandler( ParametersHandler::~ParametersHandler() { - if(auto node = node_.lock()) - { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(on_set_param_handler_.get()); } on_set_param_handler_.reset(); diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 0baaea9bc3c..70589465121 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -115,8 +115,7 @@ NavfnPlanner::deactivate() RCLCPP_INFO( logger_, "Deactivating plugin %s of type NavfnPlanner", name_.c_str()); - if(auto node = node_.lock()) - { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); 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 f1068cde00e..3a2dfea1c33 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -124,8 +124,7 @@ void RotationShimController::deactivate() primary_controller_->deactivate(); - if(auto node = node_.lock()) - { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); From f3354a3c23e918fbc32f853b0a71d98d22733d35 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 17:08:02 +0800 Subject: [PATCH 20/30] code style Signed-off-by: goes --- nav2_costmap_2d/plugins/inflation_layer.cpp | 2 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 2 +- nav2_costmap_2d/plugins/static_layer.cpp | 2 +- nav2_costmap_2d/plugins/voxel_layer.cpp | 2 +- nav2_mppi_controller/src/parameters_handler.cpp | 2 +- .../src/nav2_rotation_shim_controller.cpp | 2 +- nav2_smac_planner/src/smac_planner_2d.cpp | 3 +-- nav2_smac_planner/src/smac_planner_hybrid.cpp | 3 +-- nav2_smac_planner/src/smac_planner_lattice.cpp | 3 +-- nav2_theta_star_planner/src/theta_star_planner.cpp | 3 +-- 10 files changed, 10 insertions(+), 14 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 6b2e16d90bf..85ddd777bec 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,7 +78,7 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { - if(auto node = node_.lock()) { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index ab40e214c43..c30b1f36c59 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -62,7 +62,7 @@ namespace nav2_costmap_2d ObstacleLayer::~ObstacleLayer() { - if(auto node = node_.lock()) { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 681faa27de1..f552edca43d 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -112,7 +112,7 @@ StaticLayer::activate() void StaticLayer::deactivate() { - if(auto node = node_.lock()) { + if(auto node = node_.lock()) { 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 9011c2c433a..6230e53f8f3 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -116,7 +116,7 @@ void VoxelLayer::onInitialize() VoxelLayer::~VoxelLayer() { - if(auto node = node_.lock()) { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index 956300e073b..ef285f92151 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -28,7 +28,7 @@ ParametersHandler::ParametersHandler( ParametersHandler::~ParametersHandler() { - if(auto node = node_.lock()) { + if(auto node = node_.lock()) { node->remove_on_set_parameters_callback(on_set_param_handler_.get()); } on_set_param_handler_.reset(); 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 3a2dfea1c33..ae324bfa3cf 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -124,7 +124,7 @@ void RotationShimController::deactivate() primary_controller_->deactivate(); - if(auto node = node_.lock()) { + 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/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 207e157fd3e..083930a2bef 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -178,8 +178,7 @@ void SmacPlanner2D::deactivate() _costmap_downsampler->on_deactivate(); } // shutdown dyn_param_handler - if(auto node = _node.lock()) - { + 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/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 8679e1b1963..00eba2819bd 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -312,8 +312,7 @@ void SmacPlannerHybrid::deactivate() _costmap_downsampler->on_deactivate(); } // shutdown dyn_param_handler - if(auto node = _node.lock()) - { + 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/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index b040210bf4d..548e07f1edd 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -251,8 +251,7 @@ void SmacPlannerLattice::deactivate() _planned_footprints_publisher->on_deactivate(); } // shutdown dyn_param_handler - if(auto node = _node.lock()) - { + if(auto node = _node.lock()) { 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 156191659bf..595e7aff567 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -86,8 +86,7 @@ void ThetaStarPlanner::activate() void ThetaStarPlanner::deactivate() { RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); - if(auto node = parent_node_.lock()) - { + if(auto node = parent_node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); From 92102ade6ff1e4af271bff1f5eb515ea64c04fa5 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 17:10:01 +0800 Subject: [PATCH 21/30] code style Signed-off-by: goes --- nav2_costmap_2d/plugins/inflation_layer.cpp | 2 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 2 +- nav2_costmap_2d/plugins/static_layer.cpp | 2 +- nav2_costmap_2d/plugins/voxel_layer.cpp | 2 +- nav2_mppi_controller/src/parameters_handler.cpp | 2 +- nav2_navfn_planner/src/navfn_planner.cpp | 2 +- .../src/nav2_rotation_shim_controller.cpp | 2 +- nav2_smac_planner/src/smac_planner_2d.cpp | 2 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 2 +- nav2_smac_planner/src/smac_planner_lattice.cpp | 2 +- nav2_theta_star_planner/src/theta_star_planner.cpp | 2 +- 11 files changed, 11 insertions(+), 11 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 85ddd777bec..44a14dfefbb 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,7 +78,7 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { - if(auto node = node_.lock()) { + if (auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index c30b1f36c59..7b09b5e95f1 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -62,7 +62,7 @@ namespace nav2_costmap_2d ObstacleLayer::~ObstacleLayer() { - if(auto node = node_.lock()) { + if (auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index f552edca43d..db83b531f05 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -112,7 +112,7 @@ StaticLayer::activate() void StaticLayer::deactivate() { - if(auto node = node_.lock()) { + if (auto node = node_.lock()) { 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 6230e53f8f3..42149950acd 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -116,7 +116,7 @@ void VoxelLayer::onInitialize() VoxelLayer::~VoxelLayer() { - if(auto node = node_.lock()) { + if (auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index ef285f92151..a33ca2361a4 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -28,7 +28,7 @@ ParametersHandler::ParametersHandler( ParametersHandler::~ParametersHandler() { - if(auto node = node_.lock()) { + if (auto node = node_.lock()) { node->remove_on_set_parameters_callback(on_set_param_handler_.get()); } on_set_param_handler_.reset(); diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 70589465121..926c22adacf 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -115,7 +115,7 @@ NavfnPlanner::deactivate() RCLCPP_INFO( logger_, "Deactivating plugin %s of type NavfnPlanner", name_.c_str()); - if(auto node = node_.lock()) { + if (auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); 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 ae324bfa3cf..afb74c357eb 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -124,7 +124,7 @@ void RotationShimController::deactivate() primary_controller_->deactivate(); - if(auto node = node_.lock()) { + 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/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 083930a2bef..f624e1215aa 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -178,7 +178,7 @@ void SmacPlanner2D::deactivate() _costmap_downsampler->on_deactivate(); } // shutdown dyn_param_handler - if(auto node = _node.lock()) { + 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/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 00eba2819bd..9277c73a0ba 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -312,7 +312,7 @@ void SmacPlannerHybrid::deactivate() _costmap_downsampler->on_deactivate(); } // shutdown dyn_param_handler - if(auto node = _node.lock()) { + 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/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 548e07f1edd..c2b7abe355b 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -251,7 +251,7 @@ void SmacPlannerLattice::deactivate() _planned_footprints_publisher->on_deactivate(); } // shutdown dyn_param_handler - if(auto node = _node.lock()) { + if (auto node = _node.lock()) { 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 595e7aff567..21b70676d64 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -86,7 +86,7 @@ void ThetaStarPlanner::activate() void ThetaStarPlanner::deactivate() { RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); - if(auto node = parent_node_.lock()) { + if (auto node = parent_node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); From dab056c0e657c4924da66b6c6a29ef00226fcd87 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 18:03:51 +0800 Subject: [PATCH 22/30] code style update Signed-off-by: goes --- .../include/nav2_behavior_tree/bt_action_node.hpp | 2 +- .../test/test_constrained_smoother.cpp | 4 ++-- nav2_costmap_2d/plugins/inflation_layer.cpp | 2 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 4 +++- nav2_costmap_2d/plugins/static_layer.cpp | 4 +++- nav2_costmap_2d/plugins/voxel_layer.cpp | 4 +++- .../include/nav2_mppi_controller/tools/utils.hpp | 2 +- nav2_mppi_controller/src/parameters_handler.cpp | 4 +++- nav2_navfn_planner/src/navfn.cpp | 12 ++++++------ nav2_navfn_planner/src/navfn_planner.cpp | 4 +++- nav2_smac_planner/src/a_star.cpp | 2 +- nav2_smac_planner/src/smac_planner_2d.cpp | 4 +++- nav2_smac_planner/src/smac_planner_hybrid.cpp | 4 +++- nav2_smac_planner/src/smac_planner_lattice.cpp | 4 +++- nav2_smac_planner/test/test_node2d.cpp | 2 +- nav2_smac_planner/test/test_nodehybrid.cpp | 2 +- nav2_smac_planner/test/test_nodelattice.cpp | 2 +- 17 files changed, 39 insertions(+), 23 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 7276ef79ad2..8176c590841 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -387,7 +387,7 @@ class BtActionNode : public BT::ActionNodeBase }; send_goal_options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { + const std::shared_ptr feedback) { feedback_ = feedback; emitWakeUpSignal(); }; diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 1ca6461e9a9..6687b3452da 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -404,7 +404,7 @@ class SmootherTest : public ::testing::Test int cusp_i_ = -1; QualityCriterion3 mvmt_smoothness_criterion_ = [this](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, - const Eigen::Vector3d & next_p) { + const Eigen::Vector3d & next_p) { Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); if (i == cusp_i_) { @@ -986,7 +986,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) int cusp_i_out = 6; // for upsampled path QualityCriterion3 mvmt_smoothness_criterion_out = [&cusp_i_out](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, - const Eigen::Vector3d & next_p) { + const Eigen::Vector3d & next_p) { Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); if (i == cusp_i_out) { diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 44a14dfefbb..7b00fcc7320 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,7 +78,7 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { - if (auto node = node_.lock()) { + if (dyn_params_handler_ && auto node = node_.lock()) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 7b09b5e95f1..069f4c88a03 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -62,10 +62,12 @@ namespace nav2_costmap_2d ObstacleLayer::~ObstacleLayer() { - if (auto node = node_.lock()) { + 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(); } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index db83b531f05..590a7b046e6 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -112,10 +112,12 @@ StaticLayer::activate() void StaticLayer::deactivate() { - if (auto node = node_.lock()) { + 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 diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 42149950acd..126a53c3670 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -116,10 +116,12 @@ void VoxelLayer::onInitialize() VoxelLayer::~VoxelLayer() { - if (auto node = node_.lock()) { + 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() diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index ddde6077659..ed303157a85 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -474,7 +474,7 @@ inline void savitskyGolayFilter( auto applyFilterOverAxis = [&](xt::xtensor & sequence, - const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void { unsigned int idx = 0; sequence(idx) = applyFilter( diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index a33ca2361a4..dc14f6decab 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -28,10 +28,12 @@ ParametersHandler::ParametersHandler( ParametersHandler::~ParametersHandler() { - if (auto node = node_.lock()) { + 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() diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index d166dbc7b1b..2cce713b0f2 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -326,14 +326,14 @@ int NavFn::getPathLen() {return npath;} // inserting onto the priority blocks #define push_cur(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \ - {curP[curPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \ + {curP[curPe++] = n; pending[n] = true;}} #define push_next(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \ - {nextP[nextPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \ + {nextP[nextPe++] = n; pending[n] = true;}} #define push_over(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \ - {overP[overPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \ + {overP[overPe++] = n; pending[n] = true;}} // Set up navigation potential arrays for new propagation diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 926c22adacf..86c05709280 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -115,10 +115,12 @@ NavfnPlanner::deactivate() RCLCPP_INFO( logger_, "Deactivating plugin %s of type NavfnPlanner", name_.c_str()); - if (auto node = node_.lock()) { + 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 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 3c22ce450f0..aa95567123e 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 f624e1215aa..20ca2f12ccf 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -178,10 +178,12 @@ void SmacPlanner2D::deactivate() _costmap_downsampler->on_deactivate(); } // shutdown dyn_param_handler - if (auto node = _node.lock()) { + 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() diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 9277c73a0ba..b0eabbe18e2 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -312,10 +312,12 @@ void SmacPlannerHybrid::deactivate() _costmap_downsampler->on_deactivate(); } // shutdown dyn_param_handler - if (auto node = _node.lock()) { + 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() diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index c2b7abe355b..595dfb955ee 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -251,10 +251,12 @@ void SmacPlannerLattice::deactivate() _planned_footprints_publisher->on_deactivate(); } // shutdown dyn_param_handler - if (auto node = _node.lock()) { + 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() diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 7b99858500f..1326fb02136 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -145,7 +145,7 @@ TEST(Node2DTest, test_node_2d_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::Node2D * & neighbor_rtn) -> bool + nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { return false; }; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index fb17dad5201..394b1881e81 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -365,7 +365,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool { // because we don't return a real object return false; diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index e118388a073..2355ac9a842 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -306,7 +306,7 @@ TEST(NodeLatticeTest, test_get_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool + nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool { // because we don't return a real object return false; From 82c64793daa9a3eec4252c92f1492c343efd2ffe Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 18:14:00 +0800 Subject: [PATCH 23/30] back Signed-off-by: goes --- nav2_smac_planner/test/test_node2d.cpp | 2 +- nav2_smac_planner/test/test_nodehybrid.cpp | 2 +- nav2_smac_planner/test/test_nodelattice.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 1326fb02136..7b99858500f 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -145,7 +145,7 @@ TEST(Node2DTest, test_node_2d_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::Node2D * & neighbor_rtn) -> bool + nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { return false; }; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 394b1881e81..fb17dad5201 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -365,7 +365,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool { // because we don't return a real object return false; diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 2355ac9a842..e118388a073 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -306,7 +306,7 @@ TEST(NodeLatticeTest, test_get_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool + nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool { // because we don't return a real object return false; From b9cf37bcab1d65af8f66de77f68bb9e7c53e7449 Mon Sep 17 00:00:00 2001 From: goes Date: Thu, 18 Jul 2024 01:07:51 +0800 Subject: [PATCH 24/30] rebase conflict resovled Signed-off-by: goes --- .../include/nav2_behavior_tree/bt_action_node.hpp | 2 +- .../test/test_constrained_smoother.cpp | 4 ++-- .../include/nav2_mppi_controller/tools/utils.hpp | 2 +- nav2_navfn_planner/src/navfn.cpp | 12 ++++++------ 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 8176c590841..7276ef79ad2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -387,7 +387,7 @@ class BtActionNode : public BT::ActionNodeBase }; send_goal_options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { + const std::shared_ptr feedback) { feedback_ = feedback; emitWakeUpSignal(); }; diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 6687b3452da..1ca6461e9a9 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -404,7 +404,7 @@ class SmootherTest : public ::testing::Test int cusp_i_ = -1; QualityCriterion3 mvmt_smoothness_criterion_ = [this](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, - const Eigen::Vector3d & next_p) { + const Eigen::Vector3d & next_p) { Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); if (i == cusp_i_) { @@ -986,7 +986,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) int cusp_i_out = 6; // for upsampled path QualityCriterion3 mvmt_smoothness_criterion_out = [&cusp_i_out](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, - const Eigen::Vector3d & next_p) { + const Eigen::Vector3d & next_p) { Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); if (i == cusp_i_out) { diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index ed303157a85..ddde6077659 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -474,7 +474,7 @@ inline void savitskyGolayFilter( auto applyFilterOverAxis = [&](xt::xtensor & sequence, - const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void { unsigned int idx = 0; sequence(idx) = applyFilter( diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 2cce713b0f2..d166dbc7b1b 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -326,14 +326,14 @@ int NavFn::getPathLen() {return npath;} // inserting onto the priority blocks #define push_cur(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \ - {curP[curPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \ + {curP[curPe++] = n; pending[n] = true;}} #define push_next(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \ - {nextP[nextPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \ + {nextP[nextPe++] = n; pending[n] = true;}} #define push_over(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \ - {overP[overPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \ + {overP[overPe++] = n; pending[n] = true;}} // Set up navigation potential arrays for new propagation From d1428638ef0bd58d8bf3348bbaa4924b7b143d03 Mon Sep 17 00:00:00 2001 From: goes Date: Thu, 18 Jul 2024 01:18:15 +0800 Subject: [PATCH 25/30] rebase error fixed Signed-off-by: goes --- nav2_costmap_2d/plugins/inflation_layer.cpp | 4 +++- nav2_theta_star_planner/src/theta_star_planner.cpp | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 7b00fcc7320..6edc8928c58 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,10 +78,12 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { - if (dyn_params_handler_ && auto node = node_.lock()) { + 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_; } diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 21b70676d64..24f2b81000d 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -86,7 +86,8 @@ void ThetaStarPlanner::activate() void ThetaStarPlanner::deactivate() { RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); - if (auto node = parent_node_.lock()) { + auto node = parent_node_.lock(); + if (node && dyn_params_handler_) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); From e2e4f594406ad4722276839e5437b366854a9a3c Mon Sep 17 00:00:00 2001 From: goes Date: Thu, 18 Jul 2024 01:21:13 +0800 Subject: [PATCH 26/30] fixed2 Signed-off-by: goes --- nav2_costmap_2d/plugins/inflation_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 6edc8928c58..ff7e659b622 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -78,7 +78,7 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { - auto node = node_.lock() + auto node = node_.lock(); if (dyn_params_handler_ && node) { node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } From ea2a35869762d6711ecf5bf575c3c501a1b6db46 Mon Sep 17 00:00:00 2001 From: goes Date: Thu, 18 Jul 2024 01:30:42 +0800 Subject: [PATCH 27/30] rebase fixed 3 Signed-off-by: goes --- nav2_smac_planner/src/smac_planner_2d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 20ca2f12ccf..8c5a74c4723 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -178,7 +178,7 @@ void SmacPlanner2D::deactivate() _costmap_downsampler->on_deactivate(); } // shutdown dyn_param_handler - auto node = node_.lock(); + auto node = _node.lock(); if (_dyn_params_handler && node) { node->remove_on_set_parameters_callback(_dyn_params_handler.get()); } From f4b96eaaa0bb7942676825e82f6d579802725754 Mon Sep 17 00:00:00 2001 From: goes Date: Thu, 18 Jul 2024 01:33:55 +0800 Subject: [PATCH 28/30] 33 Signed-off-by: goes --- nav2_smac_planner/src/smac_planner_hybrid.cpp | 2 +- nav2_smac_planner/src/smac_planner_lattice.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index b0eabbe18e2..a77d5980e2a 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -312,7 +312,7 @@ void SmacPlannerHybrid::deactivate() _costmap_downsampler->on_deactivate(); } // shutdown dyn_param_handler - auto node = node_.lock(); + auto node = _node.lock(); if (_dyn_params_handler && node) { node->remove_on_set_parameters_callback(_dyn_params_handler.get()); } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 595dfb955ee..62056a56d89 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -251,7 +251,7 @@ void SmacPlannerLattice::deactivate() _planned_footprints_publisher->on_deactivate(); } // shutdown dyn_param_handler - auto node = node_.lock(); + auto node = _node.lock(); if (_dyn_params_handler && node) { node->remove_on_set_parameters_callback(_dyn_params_handler.get()); } From d2c01abf60ee2d24135836c31a4c222031fde698 Mon Sep 17 00:00:00 2001 From: GoesM Date: Fri, 19 Jul 2024 00:14:53 +0800 Subject: [PATCH 29/30] shared_ptr into weak_ptr Signed-off-by: GoesM --- nav2_costmap_2d/plugins/inflation_layer.cpp | 1 - nav2_costmap_2d/plugins/obstacle_layer.cpp | 1 - nav2_costmap_2d/plugins/static_layer.cpp | 1 - nav2_costmap_2d/plugins/voxel_layer.cpp | 1 - .../dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp | 2 +- nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp | 5 ++++- .../include/nav2_graceful_controller/parameter_handler.hpp | 2 +- nav2_graceful_controller/src/parameter_handler.cpp | 5 ++++- .../parameter_handler.hpp | 2 +- .../src/parameter_handler.cpp | 5 ++++- 10 files changed, 15 insertions(+), 10 deletions(-) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index ff7e659b622..6ce187a0b23 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -83,7 +83,6 @@ InflationLayer::~InflationLayer() node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); - node.reset(); delete access_; } diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 069f4c88a03..6321305dd79 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -67,7 +67,6 @@ ObstacleLayer::~ObstacleLayer() node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); - node.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 590a7b046e6..67b7fbd5ec9 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -117,7 +117,6 @@ StaticLayer::deactivate() node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); - node.reset(); } void diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 126a53c3670..c4344d9f0e7 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -121,7 +121,6 @@ VoxelLayer::~VoxelLayer() node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); - node.reset(); } void VoxelLayer::matchSize() 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 63918bb3b94..a87376e718c 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,7 +150,7 @@ class KinematicsHandler using Ptr = std::shared_ptr; protected: - nav2_util::LifecycleNode::SharedPtr node_; + 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 719b7feacc0..dd7d5ff1e64 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -56,7 +56,10 @@ KinematicsHandler::KinematicsHandler() KinematicsHandler::~KinematicsHandler() { - node_->remove_on_set_parameters_callback(dyn_params_handler_.get()); + 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(); } 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 cbc57848d68..7594524cf0f 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -77,7 +77,7 @@ class ParameterHandler Parameters * getParams() {return ¶ms_;} protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; /** * @brief Callback executed when a parameter change is detected diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 7eeef061ac5..3d3389ec1d9 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -106,7 +106,10 @@ ParameterHandler::ParameterHandler( ParameterHandler::~ParameterHandler() { - node_->remove_on_set_parameters_callback(dyn_params_handler_.get()); + 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 5d646c8460e..66a53123436 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 @@ -88,7 +88,7 @@ class ParameterHandler Parameters * getParams() {return ¶ms_;} protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + 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 5149844ebf1..298ff2bf93a 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -197,7 +197,10 @@ ParameterHandler::ParameterHandler( ParameterHandler::~ParameterHandler() { - node_->remove_on_set_parameters_callback(dyn_params_handler_.get()); + auto node = node_.lock(); + if (dyn_params_handler_ && node) { + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + } dyn_params_handler_.reset(); } From cb5bd22ad8898589c3344602c115288245fcdf9e Mon Sep 17 00:00:00 2001 From: GoesM Date: Fri, 19 Jul 2024 00:16:14 +0800 Subject: [PATCH 30/30] remove adundant node.resest() Signed-off-by: GoesM --- nav2_mppi_controller/src/parameters_handler.cpp | 1 - nav2_navfn_planner/src/navfn_planner.cpp | 1 - nav2_smac_planner/src/smac_planner_2d.cpp | 1 - nav2_smac_planner/src/smac_planner_hybrid.cpp | 1 - nav2_smac_planner/src/smac_planner_lattice.cpp | 1 - 5 files changed, 5 deletions(-) diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index dc14f6decab..c66d5725615 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -33,7 +33,6 @@ ParametersHandler::~ParametersHandler() node->remove_on_set_parameters_callback(on_set_param_handler_.get()); } on_set_param_handler_.reset(); - node.reset(); } void ParametersHandler::start() diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 86c05709280..7c94751a761 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -120,7 +120,6 @@ NavfnPlanner::deactivate() node->remove_on_set_parameters_callback(dyn_params_handler_.get()); } dyn_params_handler_.reset(); - node.reset(); } void diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 8c5a74c4723..728d9910fd9 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -183,7 +183,6 @@ void SmacPlanner2D::deactivate() node->remove_on_set_parameters_callback(_dyn_params_handler.get()); } _dyn_params_handler.reset(); - node.reset(); } void SmacPlanner2D::cleanup() diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index a77d5980e2a..5ba10d7d9a0 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -317,7 +317,6 @@ void SmacPlannerHybrid::deactivate() node->remove_on_set_parameters_callback(_dyn_params_handler.get()); } _dyn_params_handler.reset(); - node.reset(); } void SmacPlannerHybrid::cleanup() diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 62056a56d89..c5a9ecdff60 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -256,7 +256,6 @@ void SmacPlannerLattice::deactivate() node->remove_on_set_parameters_callback(_dyn_params_handler.get()); } _dyn_params_handler.reset(); - node.reset(); } void SmacPlannerLattice::cleanup()