clear start cell for planning (ros-navigation#3980)

* clear start cell for planning * clean up * add headers * added comment * start blocked * code review * code review
Marc-Morcos · Jul 4, 2024 · a509143 · a509143
1 parent cb9d126
commit a509143
Showing 1 changed file with 14 additions and 0 deletions.
diff --git 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>;