diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
index 6b04ca87bfb7d..1902b7aa2d091 100644
--- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
+++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
@@ -131,10 +131,7 @@ void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
 {
   AbstractPlanningAlgorithm::setMap(costmap);
 
-  clearNodes();
-
   x_scale_ = costmap_.info.height;
-  graph_.reserve(100000);
 }
 
 bool AstarSearch::makePlan(
@@ -143,6 +140,9 @@ bool AstarSearch::makePlan(
   start_pose_ = global2local(costmap_, start_pose);
   goal_pose_ = global2local(costmap_, goal_pose);
 
+  clearNodes();
+  graph_.reserve(100000);
+
   if (!setStartNode()) {
     return false;
   }