From d890bb5ed51354f81c269e876374dfa607d25bcd Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 14 May 2024 12:19:13 +0900 Subject: [PATCH 1/2] fix issue when ego does not straddle lane bounds and starts from 0 speed Signed-off-by: Daniel Sanchez --- .../src/turn_signal_decider.cpp | 5 ++++- .../src/start_planner_module.cpp | 11 +++++++++-- 2 files changed, 13 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 320cc96577187..21937aa76da29 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -731,10 +731,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/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index bee3fb16595b7..fbdb9ae6533e6 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 @@ -241,9 +241,12 @@ void StartPlannerModule::updateData() DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); } + constexpr double moving_velocity_threshold = 0.1; + const double & ego_velocity = planner_data_->self_odometry->twist.twist.linear.x; if ( planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && - status_.driving_forward && !status_.first_engaged_and_driving_forward_time) { + status_.driving_forward && !status_.first_engaged_and_driving_forward_time && + ego_velocity > moving_velocity_threshold) { status_.first_engaged_and_driving_forward_time = clock_->now(); } @@ -1265,8 +1268,12 @@ 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. + // this issue. Also, if the ego is not engaged (so it is stopped), the blinkers should still be + // activated. const bool override_ego_stopped_check = std::invoke([&]() { + if (!status_.first_engaged_and_driving_forward_time) { + return true; + } if (status_.planner_type != PlannerType::GEOMETRIC) { return false; } From 85cef5a49262bac333495cbc65f04e0c6b2a1329 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 15 May 2024 09:33:56 +0900 Subject: [PATCH 2/2] add new status for ego departed Signed-off-by: Daniel Sanchez --- .../start_planner_module.hpp | 2 ++ .../src/start_planner_module.cpp | 21 ++++++++++++------- 2 files changed, 15 insertions(+), 8 deletions(-) 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 c05b7a8f94716..591d8adb60819 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 fbdb9ae6533e6..280e1112ef7fb 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 @@ -241,15 +241,19 @@ void StartPlannerModule::updateData() DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); } - constexpr double moving_velocity_threshold = 0.1; - const double & ego_velocity = planner_data_->self_odometry->twist.twist.linear.x; if ( planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && - status_.driving_forward && !status_.first_engaged_and_driving_forward_time && - ego_velocity > moving_velocity_threshold) { + status_.driving_forward && !status_.first_engaged_and_driving_forward_time) { 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(); @@ -1270,10 +1274,8 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid // this issue. Also, if the ego is not engaged (so it is stopped), the blinkers should still be // activated. - const bool override_ego_stopped_check = std::invoke([&]() { - if (!status_.first_engaged_and_driving_forward_time) { - return true; - } + + const bool geometric_planner_has_not_finished_first_path = std::invoke([&]() { if (status_.planner_type != PlannerType::GEOMETRIC) { return false; } @@ -1284,6 +1286,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);