fix(obstacle_avoidance_planner): isPathGoalChanged (autowarefoundation#2205)

kosuke55 · HansRobo · commit bd76aeea05b0 · 2022-12-16T21:12:28.000+09:00
Signed-off-by: kosuke55 &lt;kosuke.tnp@gmail.com&gt;

Signed-off-by: kosuke55 &lt;kosuke.tnp@gmail.com&gt;
Signed-off-by: Kotaro Yoshimoto &lt;pythagora.yoshimoto@gmail.com&gt;
diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp
@@ -1001,7 +1001,7 @@ bool ObstacleAvoidancePlanner::checkReplan(const PlannerData & planner_data)
     return true;
   }
 
-  if (isPathGoalChanged(planner_data)) {
+  if (isPathGoalChanged(p)) {
     RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed.");
     resetPrevOptimization();
     return true;
@@ -1077,7 +1077,7 @@ bool ObstacleAvoidancePlanner::isPathGoalChanged(const PlannerData & planner_dat
 {
   const auto & p = planner_data;
 
-  if (prev_path_points_ptr_) {
+  if (!prev_path_points_ptr_) {
     return false;
   }
 

Original file line number	Diff line number	Diff line change
`@@ -1001,7 +1001,7 @@ bool ObstacleAvoidancePlanner::checkReplan(const PlannerData & planner_data)`
`1001`	`1001`	`return true;`
`1002`	`1002`	`}`
`1003`	`1003`
`1004`		`- if (isPathGoalChanged(planner_data)) {`
	`1004`	`+ if (isPathGoalChanged(p)) {`
`1005`	`1005`	`RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed.");`
`1006`	`1006`	`resetPrevOptimization();`
`1007`	`1007`	`return true;`
`@@ -1077,7 +1077,7 @@ bool ObstacleAvoidancePlanner::isPathGoalChanged(const PlannerData & planner_dat`
`1077`	`1077`	`{`
`1078`	`1078`	`const auto & p = planner_data;`
`1079`	`1079`
`1080`		`- if (prev_path_points_ptr_) {`
	`1080`	`+ if (!prev_path_points_ptr_) {`
`1081`	`1081`	`return false;`
`1082`	`1082`	`}`
`1083`	`1083`