From 72bd87ff8d2f919da699ab97e1d5760b4878b25e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 15 May 2024 16:40:45 +0900 Subject: [PATCH] =?UTF-8?q?fix(start=5Fplanner):=20issue=20when=20ego=20do?= =?UTF-8?q?es=20not=20straddle=20lane=20bounds=20and=20=E2=80=A6=20(#1299)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit fix(start_planner): issue when ego does not straddle lane bounds and starts from 0 speed (#7004) * fix issue when ego does not straddle lane bounds and starts from 0 speed * add new status for ego departed --------- Signed-off-by: Daniel Sanchez --- .../src/turn_signal_decider.cpp | 5 ++++- .../start_planner_module.hpp | 2 ++ .../src/start_planner_module.cpp | 16 ++++++++++++++-- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 7bb2bab20bdcd..a87a41861c9b4 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -738,10 +738,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( return std::make_pair(TurnSignalInfo{}, true); } - if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { + // Check if the ego will cross lane bounds. + // Note that pull out requires blinkers, even if the ego does not cross lane bounds + if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { return std::make_pair(TurnSignalInfo{}, true); } + // If the ego has stopped and its close to completing its shift, turn off the blinkers constexpr double STOPPED_THRESHOLD = 0.1; // [m/s] if (ego_speed < STOPPED_THRESHOLD && !override_ego_stopped_check) { if (isNearEndOfShift( diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 2d661cf41855f..927428a77c2b0 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -74,6 +74,8 @@ struct PullOutStatus //! record the first time when ego started forward-driving (maybe after backward driving //! completion) in AUTONOMOUS operation mode std::optional first_engaged_and_driving_forward_time{std::nullopt}; + // record if the ego has departed from the start point + bool has_departed{false}; PullOutStatus() {} }; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 0bd68ce05ec1d..11a72aef36ca4 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -183,6 +183,13 @@ void StartPlannerModule::updateData() status_.first_engaged_and_driving_forward_time = clock_->now(); } + constexpr double moving_velocity_threshold = 0.1; + const double & ego_velocity = planner_data_->self_odometry->twist.twist.linear.x; + if (status_.first_engaged_and_driving_forward_time && ego_velocity > moving_velocity_threshold) { + // Ego is engaged, and has moved + status_.has_departed = true; + } + status_.backward_driving_complete = hasFinishedBackwardDriving(); if (status_.backward_driving_complete) { updateStatusAfterBackwardDriving(); @@ -1212,8 +1219,10 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() // In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction. // This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid - // this issue. - const bool override_ego_stopped_check = std::invoke([&]() { + // this issue. Also, if the ego is not engaged (so it is stopped), the blinkers should still be + // activated. + + const bool geometric_planner_has_not_finished_first_path = std::invoke([&]() { if (status_.planner_type != PlannerType::GEOMETRIC) { return false; } @@ -1224,6 +1233,9 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() return distance_from_ego_to_stop_point < distance_threshold; }); + const bool override_ego_stopped_check = + !status_.has_departed || geometric_planner_has_not_finished_first_path; + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);