From 43c08e359d0599198dece403cbddb31254708e61 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 18 Jul 2023 07:58:45 +0900 Subject: [PATCH] Revert "fix(behavior_path_planner): check new modified goal in the node side (#4295)" This reverts commit 76c61de4e10fe3c0240a8f372e092e7fec24ebdd. --- .../src/behavior_path_planner_node.cpp | 7 +------ .../src/scene_module/goal_planner/goal_planner_module.cpp | 5 ++++- 2 files changed, 5 insertions(+), 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 index cfe97662954f6..7374520d30b72 100644 --- 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 index db0c3401ec994..3c4feebe35529 100644 --- 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;