cleanup

Signed-off-by: stevedan <[email protected]>
ros-navigation · Jan 23, 2025 · 6a94c0a · 6a94c0a
1 parent 5ff4de0
commit 6a94c0a
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 32 deletions.
diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
@@ -136,7 +136,7 @@ class AnalyticExpansion
   NodePtr setAnalyticPath(
     const NodePtr & node, const NodePtr & goal,
     const AnalyticExpansionNodes & expanded_nodes);
-  
+
   /**
    * @brief Sets the goals to expand to in the analytic expansion
    * @param goals_node set of goals node to plan to

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -376,18 +376,7 @@ bool AStarAlgorithm<NodeT>::createPath(
   if (goal_heading_mode != GoalHeadingMode::ALL_DIRECTION || coarse_search_resolution == 0) {
     current_coarse_search_resolution = 1;
   }
-  // print coarse search resolution 
-  // RCLCPP_INFO(
-  //           rclcpp::get_logger("AStarAlgorithm"), "coarse **************+: %d", current_coarse_search_resolution);
-
-  std::chrono::duration<double> expansion_time = 0.0s;
-
-  int number_of_expansions = 0;
-  // RCLCPP_INFO(
-  //           rclcpp::get_logger("AStarAlgorithm"),
-  //           "************************************");
-
-  _expander->setGoalsToExpand(getGoalsVector(),current_coarse_search_resolution );
+  _expander->setGoalsToExpand(getGoalsVector(), current_coarse_search_resolution);
 
   while (iterations < getMaxIterations() && !_queue.empty()) {
     // Check for planning timeout and cancel only on every Nth iteration
@@ -428,26 +417,18 @@ bool AStarAlgorithm<NodeT>::createPath(
     expansion_result = _expander->tryAnalyticExpansion(
       current_node,
       getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance);
-    number_of_expansions++;
-    auto end_expansion = steady_clock::now();
     expansion_time += end_expansion - start_expansion;
     if (expansion_result != nullptr) {
       current_node = expansion_result;
     }
 
     // 3) Check if we're at the goal, backtrace if required
     if (isGoal(current_node)) {
-      // RCLCPP_INFO(
-      // rclcpp::get_logger("AStarAlgorithm"),
-      // "Expansion time: %f, number of expansions: %d", expansion_time.count(), number_of_expansions);
       return current_node->backtracePath(path);
     } else if (_best_heuristic_node.first < getToleranceHeuristic()) {
       // Optimization: Let us find when in tolerance and refine within reason
       approach_iterations++;
       if (approach_iterations >= getOnApproachMaxIterations()) {
-      //    RCLCPP_INFO(
-      // rclcpp::get_logger("AStarAlgorithm"),
-      // "Expansion time: %f, number of expansions: %d", expansion_time.count(), number_of_expansions);
         return _graph.at(_best_heuristic_node.second).backtracePath(path);
       }
     }
@@ -477,9 +458,6 @@ bool AStarAlgorithm<NodeT>::createPath(
 
   if (_best_heuristic_node.first < getToleranceHeuristic()) {
     // If we run out of search options, return the path that is closest, if within tolerance.
-     RCLCPP_INFO(
-      rclcpp::get_logger("AStarAlgorithm"),
-      "Expansion time: %f, number of expansions: %d", expansion_time.count(), number_of_expansions);
     return _graph.at(_best_heuristic_node.second).backtracePath(path);
   }
 

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -82,7 +82,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     analytic_iterations =
       std::min(analytic_iterations, desired_iterations);
 
-    
+
     if (analytic_iterations <= 0) {
       analytic_iterations = desired_iterations;
 
@@ -105,7 +105,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
           current_best_analytic_nodes = analytic_nodes;
           current_best_goal = current_goal_node;
           current_best_node = node;
-          found_valid_expansion = true;          
+          found_valid_expansion = true;
           break;
         }
         number_of_checked_goal++;
@@ -133,10 +133,10 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
               current_best_node = node;
             }
           }
-        }    
+        }
       }
     }
-  
+
     if (!current_best_analytic_nodes.empty()) {
       return setAnalyticPath(
         current_best_node, current_best_goal,
@@ -441,7 +441,9 @@ typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyt
 }
 
 template<typename NodeT>
-void AnalyticExpansion<NodeT>::setGoalsToExpand(const NodeVector & goals_node, const int & coarse_search_resolution)
+void AnalyticExpansion<NodeT>::setGoalsToExpand(
+  const NodeVector & goals_node,
+  const int & coarse_search_resolution)
 {
   // clear the goals to expand
   _goals_to_expand.clear();
@@ -454,7 +456,7 @@ void AnalyticExpansion<NodeT>::setGoalsToExpand(const NodeVector & goals_node, c
   }
   // add the rest of the goals
   unsigned int rest_index = _coarse_search_goal_size;
-  if(coarse_search_resolution != 1){
+  if(coarse_search_resolution != 1) {
     for (unsigned int i = 0; i < goals_node.size(); i++) {
       if (i % coarse_search_resolution != 0) {
         _goals_to_expand[rest_index] = goals_node[i];
@@ -465,9 +467,8 @@ void AnalyticExpansion<NodeT>::setGoalsToExpand(const NodeVector & goals_node, c
 }
 
 template<>
-void AnalyticExpansion<Node2D>::setGoalsToExpand(const NodeVector &, const int & )
+void AnalyticExpansion<Node2D>::setGoalsToExpand(const NodeVector &, const int &)
 {
-
 }
 
 template class AnalyticExpansion<Node2D>;