code style update

Signed-off-by: goes <GoesM@buaa.edu.cn>
ros-navigation · Jul 17, 2024 · 521e42c · 521e42c
1 parent e167aa4
commit 521e42c
Show file tree

Hide file tree

Showing 17 changed files with 39 additions and 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
@@ -387,7 +387,7 @@ class BtActionNode : public BT::ActionNodeBase
  };
  send_goal_options.feedback_callback =
  [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
- const std::shared_ptr<const typename ActionT::Feedback> feedback) {
+  const std::shared_ptr<const typename ActionT::Feedback> feedback) {
  feedback_ = feedback;
  emitWakeUpSignal();
  };

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -474,7 +474,7 @@ inline void savitskyGolayFilter(
 
  auto applyFilterOverAxis =
  [&](xt::xtensor<float, 1> & 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
@@ -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
@@ -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
@@ -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
@@ -71,7 +71,7 @@ void AStarAlgorithm<NodeT>::initialize(
  _max_on_approach_iterations = max_on_approach_iterations;
  _terminal_checking_interval = terminal_checking_interval;
  _max_planning_time = max_planning_time;
- if(!_is_initialized) {
+ if (!_is_initialized) {
  NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
  }
  _is_initialized = true;

diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp
@@ -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
@@ -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
@@ -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
@@ -145,7 +145,7 @@ TEST(Node2DTest, test_node_2d_neighbors)
  std::function<bool(const uint64_t &,
  nav2_smac_planner::Node2D * &)> 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
@@ -365,7 +365,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors)
  std::function<bool(const uint64_t &,
  nav2_smac_planner::NodeHybrid * &)> 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
@@ -306,7 +306,7 @@ TEST(NodeLatticeTest, test_get_neighbors)
  std::function<bool(const uint64_t &,
  nav2_smac_planner::NodeLattice * &)> 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;