From 62989068ef2cd27bf88a1605f8279530ebc7de2d Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 17 May 2024 17:03:22 +0900 Subject: [PATCH] fix(avoidance): don't output turn signal if there is huge lateral deveation Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index d884c22e71c44..b536e8c748716 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -913,11 +913,21 @@ BehaviorModuleOutput AvoidanceModule::plan() ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; }; + const auto is_large_deviation = [this](const auto & path) { + constexpr double THRESHOLD = 1.0; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto lateral_deviation = + motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); + return std::abs(lateral_deviation) > THRESHOLD; + }; + // turn signal info if (path_shifter_.getShiftLines().empty()) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else if (is_large_deviation(spline_shift_path.path)) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info;