From 1deb568c30790cefb37cc8efe2291239e6a671a6 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 17 Jul 2023 11:39:40 +0900 Subject: [PATCH] fix(behavior_path_planner): check new modified goal in the node side Signed-off-by: kosuke55 --- .../src/behavior_path_planner_node.cpp | 7 ++++++- .../src/scene_module/goal_planner/goal_planner_module.cpp | 5 +---- 2 files changed, 7 insertions(+), 5 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 7374520d30b72..cfe97662954f6 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -571,7 +571,12 @@ void BehaviorPathPlannerNode::run() publishPathReference(planner_manager_->getSceneModuleManagers(), planner_data_); stop_reason_publisher_->publish(planner_manager_->getStopReasons()); - if (output.modified_goal) { + // 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)) { 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 3c4feebe35529..db0c3401ec994 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,11 +677,8 @@ 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; - 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) { + if (status_.is_safe) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); modified_goal.pose = modified_goal_pose_->goal_pose;