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 56e5bbccaf..64c905d01c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -250,12 +250,6 @@ class AStarAlgorithm inline void populateExpansionsLog( const NodePtr & node, std::vector> * expansions_log); - /** - * @brief Clear Start - */ - void clearStart(); - - bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 8c8ec7197f..a3cfbb4b4f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -250,8 +250,6 @@ bool AStarAlgorithm::areInputsValid() } // Note: We do not check the if the start is valid because it is cleared - clearStart(); - return true; } @@ -484,20 +482,6 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } -template<> -void AStarAlgorithm::clearStart() -{ - auto coords = Node2D::getCoords(_start->getIndex()); - _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); -} - -template -void AStarAlgorithm::clearStart() -{ - auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); - _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); -} - // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm;