improvement

Signed-off-by: stevedan <[email protected]>
ros-navigation · Mar 4, 2024 · 101f345 · 101f345
1 parent 6f315fe
commit 101f345
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 8 deletions.
diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
@@ -3,7 +3,6 @@ project(nav2_smac_planner)
 
 set(CMAKE_BUILD_TYPE Release) #Debug, Release
 
-
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -63,7 +63,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
 
     float current_best_score = std::numeric_limits<float>::max();
     AnalyticExpansionNodes current_best_analytic_nodes = {};
-    NodePtr current_best_goal_node = nullptr;
+    unsigned int current_best_goal_index = 0;
     NodePtr current_best_node = nullptr;
     for (unsigned int i = 0; i < goals_node.size(); i++) {
       closest_distance = std::min(
@@ -159,7 +159,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
           {
             current_best_score = best_score;
             current_best_node = node;
-            current_best_goal_node = goals_node[i];
+            current_best_goal_index = i;
             current_best_analytic_nodes = analytic_nodes;
           }
 
@@ -168,7 +168,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     }
     if(!current_best_analytic_nodes.empty())
     {
-      return setAnalyticPath(current_best_node, current_best_goal_node, current_best_analytic_nodes);
+      return setAnalyticPath(current_best_node, goals_node[current_best_goal_index], current_best_analytic_nodes);
     }
     analytic_iterations--;   
   }

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -177,7 +177,10 @@ void SmacPlannerHybrid::configure(
       "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ";
     throw nav2_core::PlannerException(error_msg);
   }
-  _goal_heading_mode = goal_heading_mode;
+  else 
+  {
+      _goal_heading_mode = goal_heading_mode;
+  }
 
   _motion_model = fromString(_motion_model_for_search);
   if (_motion_model == MotionModel::UNKNOWN) {
@@ -650,7 +653,10 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
             "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ",
             goal_heading_type.c_str());
         }
-        _goal_heading_mode = goal_heading_mode;
+        else 
+        {
+          _goal_heading_mode = goal_heading_mode;
+        }
       }
     }
   }

diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -153,7 +153,10 @@ void SmacPlannerLattice::configure(
     "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ";
     throw nav2_core::PlannerException(error_msg);
   }
-  _goal_heading_mode = goal_heading_mode;
+  else 
+  {
+      _goal_heading_mode = goal_heading_mode;
+  }
 
   _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
   _search_info.minimum_turning_radius =
@@ -560,7 +563,10 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
             "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ",
             goal_heading_type.c_str());
         }
-        _goal_heading_mode = goal_heading_mode;
+        else 
+        {
+          _goal_heading_mode = goal_heading_mode;
+        }
       }
     }
   }