Revert "fix(behavior_path_planner): check new modified goal in the no…

…de side (#4295)" This reverts commit 76c61de.
autowarefoundation · Jul 17, 2023 · 43c08e3 · 43c08e3
1 parent 361fbaf
commit 43c08e3
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 7 deletions.
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -571,12 +571,7 @@ void BehaviorPathPlannerNode::run()
   publishPathReference(planner_manager_->getSceneModuleManagers(), planner_data_);
   stop_reason_publisher_->publish(planner_manager_->getStopReasons());
 
-  // publish modified goal only when it is updated
-  if (
-    output.modified_goal &&
-    /* has changed modified goal */ (
-      !planner_data_->prev_modified_goal ||
-      planner_data_->prev_modified_goal->uuid != output.modified_goal->uuid)) {
+  if (output.modified_goal) {
     PoseWithUuidStamped modified_goal = *(output.modified_goal);
     modified_goal.header.stamp = path->header.stamp;
     planner_data_->prev_modified_goal = modified_goal;

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -677,8 +677,11 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 
 void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const
 {
+  // set the modified goal only when it is updated
   const auto & route_handler = planner_data_->route_handler;
-  if (status_.is_safe) {
+  const bool has_changed_goal =
+    modified_goal_pose_ && (!prev_goal_id_ || *prev_goal_id_ != modified_goal_pose_->id);
+  if (status_.is_safe && has_changed_goal) {
     PoseWithUuidStamped modified_goal{};
     modified_goal.uuid = route_handler->getRouteUuid();
     modified_goal.pose = modified_goal_pose_->goal_pose;