From 521e42c34ddfb12abca4abbef98a6dd5bd21b340 Mon Sep 17 00:00:00 2001 From: goes Date: Wed, 17 Jul 2024 18:03:51 +0800 Subject: [PATCH] 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;