From 738df1fa851bff6d9fe4bd128c15f6431915f497 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 17 Jul 2023 13:56:49 +0900 Subject: [PATCH] fix(behavior_path_planner): check new modified goal in the node side (#4295) 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 23ff1180753ab..432242230a3df 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1294,7 +1294,12 @@ void BehaviorPathPlannerNode::run() lk_manager.unlock(); // release bt_manager_ #endif - 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 0f054c4a67d38..e9c15fc7cf0bc 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 @@ -689,11 +689,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;