From 3c230ebc0744cf93e9669f87f4e12cf89cf9b120 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 19 Sep 2024 18:46:22 +0900 Subject: [PATCH] fix(lane_change): set initail rtc state properly (#8902) set initail rtc state properly Signed-off-by: Go Sakayori --- .../src/interface.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 61daf1183e993..7ddafdbd6c935 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -125,16 +125,21 @@ BehaviorModuleOutput LaneChangeInterface::plan() } else { const auto path = assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); - const auto force_activated = std::any_of( + const auto is_registered = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), - [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); - if (!force_activated) { + [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)); }); + + if (!is_registered) { updateRTCStatus( path.start_distance_to_path_change, path.finish_distance_to_path_change, true, - State::RUNNING); + State::WAITING_FOR_EXECUTION); } else { + const auto force_activated = std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); + const bool safe = force_activated ? false : true; updateRTCStatus( - path.start_distance_to_path_change, path.finish_distance_to_path_change, false, + path.start_distance_to_path_change, path.finish_distance_to_path_change, safe, State::RUNNING); } }