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; }