From a6cacac319e7c7a910d0ed5959f63016ecafbcc1 Mon Sep 17 00:00:00 2001 From: Vincent <46542431+VincidaB@users.noreply.github.com> Date: Wed, 19 Jun 2024 10:37:41 -0500 Subject: [PATCH] precomputeDistanceHeuristic is now computed once (#4451) Signed-off-by: Vincent Belpois Co-authored-by: SiddharthaUpase --- nav2_smac_planner/include/nav2_smac_planner/a_star.hpp | 1 + nav2_smac_planner/src/a_star.cpp | 6 +++++- 2 files changed, 6 insertions(+), 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 index 64c905d01c..c95d8c82cf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -251,6 +251,7 @@ class AStarAlgorithm const NodePtr & node, std::vector> * expansions_log); bool _traverse_unknown; + bool _is_initialized; int _max_iterations; int _max_on_approach_iterations; int _terminal_checking_interval; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index a3cfbb4b4f..3c22ce450f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -36,6 +36,7 @@ AStarAlgorithm::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::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>( _motion_model, _search_info, _traverse_unknown, _dim3_size);