diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 2808a9cc3b8cc..d79741fe66c2e 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -258,20 +258,51 @@ void RTCInterface::updateCooperateStatus( status.module = module_; status.safe = safe; status.command_status.type = Command::DEACTIVATE; - status.state.type = state; + status.state.type = State::WAITING_FOR_EXECUTION; status.start_distance = start_distance; status.finish_distance = finish_distance; status.auto_mode = is_auto_mode_enabled_; registered_status_.statuses.push_back(status); + + if (state != State::WAITING_FOR_EXECUTION) + RCLCPP_WARN_STREAM( + getLogger(), "[updateCooperateStatus] Cannot register " + << state_to_string(state) << " as initial state" << std::endl); + + return; + } + + auto update_status = [&](auto & status) { + status.stamp = stamp; + status.safe = safe; + status.state.type = state; + status.start_distance = start_distance; + status.finish_distance = finish_distance; + }; + + // If the registered status is found, update status by considering the state transition + if ( + itr->state.type == State::WAITING_FOR_EXECUTION && + (state == State::WAITING_FOR_EXECUTION || state == State::RUNNING || state == State::FAILED)) { + update_status(*itr); + return; + } + + if (itr->state.type == State::RUNNING && state != State::WAITING_FOR_EXECUTION) { + update_status(*itr); return; } - // If the registered status is found, update status - itr->stamp = stamp; - itr->safe = safe; - itr->state.type = state; - itr->start_distance = start_distance; - itr->finish_distance = finish_distance; + if (itr->state.type == State::ABORTING && (state == State::ABORTING || state == State::FAILED)) { + update_status(*itr); + return; + } + + RCLCPP_WARN_STREAM( + getLogger(), "[updateCooperateStatus] uuid : " << uuid_to_string(uuid) + << " cannot transit from " + << state_to_string(itr->state.type) << " to " + << state_to_string(state) << std::endl); } void RTCInterface::removeCooperateStatus(const UUID & uuid) 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 982cdcf3e78a0..7f30f34a8f633 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 @@ -160,9 +160,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), false, - State::FAILED); path_candidate_ = std::make_shared(); return out; }