From 39d406ec7e011a43431ddc3625ca24a21b04f356 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 28 May 2024 20:19:58 +0900 Subject: [PATCH] fix(avoidance): fix RTC state transition (#7142) * fix(avoidance): fix RTC state transition Signed-off-by: satoshi-ota * fix(rtc_interface): check if the cooperate status is registered Signed-off-by: satoshi-ota * refactor(avoidance): rebuild avoidance module state Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../data_structs.hpp | 12 +-- .../behavior_path_avoidance_module/scene.hpp | 36 ++++---- .../src/scene.cpp | 90 ++++++++----------- .../interface/scene_module_interface.hpp | 9 +- 4 files changed, 68 insertions(+), 79 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 7958a8a2dcbd4..2e432e093583f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -501,11 +501,9 @@ using AvoidOutlines = std::vector; * avoidance state */ enum class AvoidanceState { - NOT_AVOID = 0, - AVOID_EXECUTE, - YIELD, - AVOID_PATH_READY, - AVOID_PATH_NOT_READY, + RUNNING = 0, + CANCEL, + SUCCEEDED, }; /* @@ -514,7 +512,7 @@ enum class AvoidanceState { struct AvoidancePlanningData { // ego final state - AvoidanceState state{AvoidanceState::NOT_AVOID}; + AvoidanceState state{AvoidanceState::RUNNING}; // un-shifted pose (for current lane detection) Pose reference_pose; @@ -566,8 +564,6 @@ struct AvoidancePlanningData bool ready{false}; - bool success{false}; - bool comfortable{false}; bool avoid_required{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 194e5ecdaf86c..da5e29ef4d5e9 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -64,7 +65,14 @@ class AvoidanceModule : public SceneModuleInterface std::shared_ptr get_debug_msg_array() const; private: - bool isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const; + /** + * @brief return the result whether the module can stop path generation process. + * @param avoidance data. + * @return it will return AvoidanceState::RUNNING when there are obstacles ego should avoid. + * it will return AvoidanceState::CANCEL when all obstacles have gone. + * it will return AvoidanceState::SUCCEEDED when the ego avoid all obstacles. + */ + AvoidanceState getCurrentModuleState(const AvoidancePlanningData & data) const; bool canTransitSuccessState() override; @@ -188,14 +196,6 @@ class AvoidanceModule : public SceneModuleInterface */ void updateRTCData(); - // ego state check - - /** - * @brief update ego status based on avoidance path and surround condition. - * @param ego status. (NOT_AVOID, AVOID, YIELD, AVOID_EXECUTE, AVOID_PATH_NOT_READY) - */ - AvoidanceState updateEgoState(const AvoidancePlanningData & data) const; - // ego behavior update /** @@ -359,7 +359,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief reset registered shift lines. * @details reset only when the base offset is zero. Otherwise, sudden steering will be caused; */ - void removeRegisteredShiftLines() + void removeRegisteredShiftLines(const uint8_t state) { constexpr double threshold = 0.1; if (std::abs(path_shifter_.getBaseOffset()) > threshold) { @@ -370,15 +370,19 @@ class AvoidanceModule : public SceneModuleInterface unlockNewModuleLaunch(); for (const auto & left_shift : left_shift_array_) { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - left_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), clock_->now()); + if (rtc_interface_ptr_map_.at("left")->isRegistered(left_shift.uuid)) { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, true, state, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } } for (const auto & right_shift : right_shift_array_) { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - right_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), clock_->now()); + if (rtc_interface_ptr_map_.at("right")->isRegistered(right_shift.uuid)) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, true, state, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } } if (!path_shifter_.getShiftLines().empty()) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 503438f9f7feb..9247d9b3d200a 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -115,14 +115,14 @@ bool AvoidanceModule::isExecutionReady() const return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } -bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const +AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningData & data) const { const bool has_avoidance_target = std::any_of( data.target_objects.begin(), data.target_objects.end(), [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); }); if (has_avoidance_target) { - return false; + return AvoidanceState::RUNNING; } // If the ego is on the shift line, keep RUNNING. @@ -133,7 +133,7 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & }; for (const auto & shift_line : path_shifter_.getShiftLines()) { if (within(shift_line, idx)) { - return false; + return AvoidanceState::RUNNING; } } } @@ -142,17 +142,21 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_execution_threshold; + if (has_base_offset) { + return AvoidanceState::RUNNING; + } + // Nothing to do. -> EXIT. - if (!has_shift_point && !has_base_offset) { - return true; + if (!has_shift_point) { + return AvoidanceState::SUCCEEDED; } // Be able to canceling avoidance path. -> EXIT. if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) { - return true; + return AvoidanceState::CANCEL; } - return false; + return AvoidanceState::RUNNING; } bool AvoidanceModule::canTransitSuccessState() @@ -183,7 +187,7 @@ bool AvoidanceModule::canTransitSuccessState() } } - return data.success; + return data.state == AvoidanceState::CANCEL || data.state == AvoidanceState::SUCCEEDED; } void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) @@ -502,7 +506,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de void AvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - data.success = isSatisfiedSuccessCondition(data); + data.state = getCurrentModuleState(data); /** * Find the nearest object that should be avoid. When the ego follows reference path, @@ -633,27 +637,6 @@ void AvoidanceModule::fillDebugData( } } -AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const -{ - if (data.yield_required) { - return AvoidanceState::YIELD; - } - - if (!data.avoid_required) { - return AvoidanceState::NOT_AVOID; - } - - if (!data.found_avoidance_path) { - return AvoidanceState::AVOID_PATH_NOT_READY; - } - - if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) { - return AvoidanceState::AVOID_PATH_READY; - } - - return AvoidanceState::AVOID_EXECUTE; -} - void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path) { if (parameters_->disable_path_update) { @@ -663,29 +646,30 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif insertPrepareVelocity(path); insertAvoidanceVelocity(path); - switch (data.state) { - case AvoidanceState::NOT_AVOID: { - break; - } - case AvoidanceState::YIELD: { + const auto insert_velocity = [this, &data, &path]() { + if (data.yield_required) { insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; + return; } - case AvoidanceState::AVOID_PATH_NOT_READY: { - insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; + + if (!data.avoid_required) { + return; } - case AvoidanceState::AVOID_PATH_READY: { + + if (!data.found_avoidance_path) { insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; + return; } - case AvoidanceState::AVOID_EXECUTE: { - insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); - break; + + if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) { + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); + return; } - default: - throw std::domain_error("invalid behavior"); - } + + insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); + }; + + insert_velocity(); insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); @@ -869,12 +853,16 @@ BehaviorModuleOutput AvoidanceModule::plan() updatePathShifter(data.safe_shift_line); - if (data.success) { - removeRegisteredShiftLines(); + if (data.state == AvoidanceState::SUCCEEDED) { + removeRegisteredShiftLines(State::SUCCEEDED); + } + + if (data.state == AvoidanceState::CANCEL) { + removeRegisteredShiftLines(State::FAILED); } if (data.yield_required) { - removeRegisteredShiftLines(); + removeRegisteredShiftLines(State::FAILED); } // generate path with shift points that have been inserted. @@ -941,8 +929,6 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path.path, parameters_->resample_interval_for_output); } - avoid_data_.state = updateEgoState(data); - // update output data { updateEgoBehavior(data, spline_shift_path); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 1035aff5f7093..b6dc2997c11e8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -508,9 +508,12 @@ class SceneModuleInterface { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { if (ptr) { - ptr->updateCooperateStatus( - uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), clock_->now()); + if (ptr->isRegistered(uuid_map_.at(module_name))) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), true, State::SUCCEEDED, + std::numeric_limits::lowest(), std::numeric_limits::lowest(), + clock_->now()); + } } } }