From e0ac3ac5ede9f466a848f112129d5e2255d0df52 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 | 22 +++++++++++++++---- .../src/utils/utils.cpp | 10 +++++++-- planning/rtc_interface/src/rtc_interface.cpp | 22 +++++++++++++++---- 4 files changed, 56 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 2567787a3f2e4..1669842117f9f 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -117,13 +117,18 @@ 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 = + assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, true, + State::RUNNING); } return output; @@ -147,7 +152,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; } @@ -156,7 +163,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; @@ -239,6 +247,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; } @@ -249,6 +260,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 ab9a0b7176fdb..4eccd00d78c09 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 ea80dc9bb9d1f..77fe29b41d893 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -153,7 +153,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) @@ -175,8 +184,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; + } } } } @@ -219,7 +230,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_; @@ -291,6 +302,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 {