precomputeDistanceHeuristic is now computed once (#4451)

Signed-off-by: Vincent Belpois <[email protected]> Co-authored-by: SiddharthaUpase <[email protected]>
ros-navigation · Aug 23, 2024 · 7309106 · 7309106
1 parent b3fce7b
commit 7309106
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 1 deletion.
diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -235,6 +235,7 @@ class AStarAlgorithm
   int _timing_interval = 5000;
 
   bool _traverse_unknown;
+  bool _is_initialized;
   int _max_iterations;
   int _max_on_approach_iterations;
   double _max_planning_time;

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -36,6 +36,7 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   const MotionModel & motion_model,
   const SearchInfo & search_info)
 : _traverse_unknown(true),
+  _is_initialized(false),
   _max_iterations(0),
   _max_planning_time(0),
   _x_size(0),
@@ -67,7 +68,10 @@ void AStarAlgorithm<NodeT>::initialize(
   _max_iterations = max_iterations;
   _max_on_approach_iterations = max_on_approach_iterations;
   _max_planning_time = max_planning_time;
-  NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
+  if(!_is_initialized) {
+    NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
+  }
+  _is_initialized = true;
   _dim3_size = dim_3_size;
   _expander = std::make_unique<AnalyticExpansion<NodeT>>(
     _motion_model, _search_info, _traverse_unknown, _dim3_size);