modify astar for standalone running

Takumi Ito · Takumi Ito · commit a8b417ba5534 · 2024-05-23T08:42:08.000+09:00
move clearNoe() from setMap to makePlan().

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;
diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp
@@ -129,9 +129,7 @@ AstarSearch::AstarSearch(
 void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
 {
   AbstractPlanningAlgorithm::setMap(costmap);
-
-  clearNodes();
-
+  
   x_scale_ = costmap_.info.height;
   graph_.reserve(100000);
 }
@@ -142,6 +140,8 @@ bool AstarSearch::makePlan(
   start_pose_ = global2local(costmap_, start_pose);
   goal_pose_ = global2local(costmap_, goal_pose);
 
+  clearNodes();
+
   if (!setStartNode()) {
     return false;
   }

Original file line number	Diff line number	Diff line change
`@@ -129,9 +129,7 @@ AstarSearch::AstarSearch(`
`129`	`129`	`void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap)`
`130`	`130`	`{`
`131`	`131`	`AbstractPlanningAlgorithm::setMap(costmap);`
`132`		`-`
`133`		`- clearNodes();`
`134`		`-`
	`132`	`+`
`135`	`133`	`x_scale_ = costmap_.info.height;`
`136`	`134`	`graph_.reserve(100000);`
`137`	`135`	`}`
`@@ -142,6 +140,8 @@ bool AstarSearch::makePlan(`
`142`	`140`	`start_pose_ = global2local(costmap_, start_pose);`
`143`	`141`	`goal_pose_ = global2local(costmap_, goal_pose);`
`144`	`142`
	`143`	`+ clearNodes();`
	`144`	`+`
`145`	`145`	`if (!setStartNode()) {`
`146`	`146`	`return false;`
`147`	`147`	`}`