From 62699129eb7fef84b07717e09bb7727b990b3b7a Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 20 Mar 2024 10:14:30 +0900 Subject: [PATCH] fix(avoidance): don't insert stop point when the path is invalid Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/scene.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 8de05515fe09e..12c59daef93ad 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1524,6 +1524,11 @@ void AvoidanceModule::insertWaitPoint( { const auto & data = avoid_data_; + // If avoidance path is NOT valid, don't insert any stop points. + if (!data.valid) { + return; + } + if (!data.stop_target_object) { return; } @@ -1614,6 +1619,16 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { const auto & data = avoid_data_; + // If avoidance path is NOT safe, don't insert any slow down sections. + if (!data.safe && !data.stop_target_object) { + return; + } + + // If avoidance path is NOT safe, don't insert any slow down sections. + if (!data.valid) { + return; + } + // do nothing if there is no avoidance target. if (data.target_objects.empty()) { return;