From a509143e31f4ac43aae19b3dad1c5623a05526e7 Mon Sep 17 00:00:00 2001
From: Joshua Wallace <josho.wallace@gmail.com>
Date: Wed, 6 Dec 2023 17:24:01 -0500
Subject: [PATCH] clear start cell for planning (#3980)

* clear start cell for planning

* clean up

* add headers

* added comment

* start blocked

* code review

* code review
---
 nav2_smac_planner/src/a_star.cpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
index 111130f4f0..6b6b76d931 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -510,6 +510,20 @@ unsigned int & AStarAlgorithm<NodeT>::getSizeDim3()
   return _dim3_size;
 }
 
+template<>
+void AStarAlgorithm<Node2D>::clearStart()
+{
+  auto coords = Node2D::getCoords(_start->getIndex());
+  _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE);
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::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<Node2D>;
 template class AStarAlgorithm<NodeHybrid>;