precomputeDistanceHeuristic is now computed once (ros-navigation#4451)

Signed-off-by: Vincent Belpois <[email protected]>
Co-authored-by: SiddharthaUpase <[email protected]>

Author: 2 people

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -251,6 +251,7 @@ class AStarAlgorithm
     const NodePtr & node, std::vector<std::tuple<float, float, float>> * expansions_log);
 
   bool _traverse_unknown;
+  bool _is_initialized;
   int _max_iterations;
   int _max_on_approach_iterations;
   int _terminal_checking_interval;

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),
   _terminal_checking_interval(5000),
   _max_planning_time(0),
@@ -70,7 +71,10 @@ void AStarAlgorithm<NodeT>::initialize(
   _max_on_approach_iterations = max_on_approach_iterations;
   _terminal_checking_interval = terminal_checking_interval;
   _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);