From c3826e4f08fa80e4ab6efce879fd9536a9e0adc4 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 31 May 2024 15:36:22 +0900 Subject: [PATCH] feat(lane_change): apply new RTC state (#7152) * feat(lane_change): support for new RTC state transition Signed-off-by: Fumiya Watanabe * fix: distance at aborting Signed-off-by: Fumiya Watanabe * fix(lane_change): empty check Signed-off-by: Fumiya Watanabe * fix(rtc_interface):update activation depends on RTC state Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../interface.hpp | 12 ++++++++++ .../src/interface.cpp | 17 ++++++++++---- .../src/utils/utils.cpp | 10 +++++++-- planning/rtc_interface/src/rtc_interface.cpp | 22 +++++++++++++++---- 4 files changed, 51 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 6e40f880467fd..4c3d129ac687e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -125,6 +125,18 @@ class LaneChangeInterface : public SceneModuleInterface ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; }; + void updateRTCStatus( + const double start_distance, const double finish_distance, const bool safe, + const uint8_t & state) + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), safe, state, start_distance, finish_distance, clock_->now()); + } + } + } + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index a7e4afc3a44b8..00de3ccf6b70b 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -117,11 +117,11 @@ BehaviorModuleOutput LaneChangeInterface::plan() updateSteeringFactorPtr(output); if (module_type_->isAbortState()) { waitApproval(); - removeRTCStatus(); const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::ABORTING); } else { clearWaitingApproval(); const auto path = @@ -161,7 +161,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { - removeRTCStatus(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); path_candidate_ = std::make_shared(); return out; } @@ -170,7 +172,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, + isExecutionReady(), State::WAITING_FOR_EXECUTION); is_abort_path_approved_ = false; return out; @@ -262,6 +265,9 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->isStoppedAtRedTrafficLight()) { log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); module_type_->toCancelState(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } @@ -272,6 +278,9 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->isAbleToReturnCurrentLane()) { log_debug_throttled("It's possible to return to current lane. Cancel lane change."); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } } diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 12b660497f48c..cc84176184543 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -646,8 +646,14 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path) { - const auto start_idx = path.info.shift_line.start_idx; - const auto end_idx = path.info.shift_line.end_idx; + if (path.shifted_path.shift_length.empty()) { + return 0.0; + } + + const auto start_idx = + std::min(path.info.shift_line.start_idx, path.shifted_path.shift_length.size() - 1); + const auto end_idx = + std::min(path.info.shift_line.end_idx, path.shifted_path.shift_length.size() - 1); return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 9e3e810b68e17..c3e74952f536b 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -152,7 +152,16 @@ std::vector RTCInterface::validateCooperateCommands( registered_status_.statuses.begin(), registered_status_.statuses.end(), [command](auto & s) { return s.uuid == command.uuid; }); if (itr != registered_status_.statuses.end()) { - response.success = true; + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + response.success = true; + } else { + RCLCPP_WARN_STREAM( + getLogger(), "[validateCooperateCommands] uuid : " + << to_string(command.uuid) + << " state is not WAITING_FOR_EXECUTION or RUNNING. state : " + << itr->state.type << std::endl); + response.success = false; + } } else { RCLCPP_WARN_STREAM( getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid) @@ -174,8 +183,10 @@ void RTCInterface::updateCooperateCommandStatus(const std::vectorcommand_status = command.command; - itr->auto_mode = false; + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + itr->command_status = command.command; + itr->auto_mode = false; + } } } } @@ -218,7 +229,7 @@ void RTCInterface::updateCooperateStatus( status.module = module_; status.safe = safe; status.command_status.type = Command::DEACTIVATE; - status.state.type = State::WAITING_FOR_EXECUTION; + status.state.type = state; status.start_distance = start_distance; status.finish_distance = finish_distance; status.auto_mode = is_auto_mode_enabled_; @@ -280,6 +291,9 @@ bool RTCInterface::isActivated(const UUID & uuid) const [uuid](auto & s) { return s.uuid == uuid; }); if (itr != registered_status_.statuses.end()) { + if (itr->state.type == State::FAILED || itr->state.type == State::SUCCEEDED) { + return false; + } if (itr->auto_mode) { return itr->safe; } else {