From 009341f6d2c798a0fab71a6a9ebba4fb7ee07a1c Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 1 Feb 2024 17:23:20 +0900 Subject: [PATCH] feat(intersection_occlusion)!: react RTC disapproval and stop even if occlusion detection is OFF (#6279) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 3f6136673a44a..3cf243b7893fc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -735,9 +735,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if ( - !rtc_occlusion_approved && decision_result.occlusion_stopline_idx && - planner_param.occlusion.enable) { + if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { const auto occlusion_stopline_idx = decision_result.occlusion_stopline_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -814,7 +812,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -857,7 +855,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { if (planner_param.occlusion.creep_during_peeking.enable) { const size_t occlusion_peeking_stopline = decision_result.occlusion_stopline_idx; const size_t closest_idx = decision_result.closest_idx; @@ -895,7 +893,7 @@ void reactRTCApprovalByDecisionResult( "PeekingTowardOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required ? decision_result.first_attention_stopline_idx @@ -965,7 +963,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.temporal_stop_before_attention_required ? decision_result.first_attention_stopline_idx : decision_result.occlusion_stopline_idx; @@ -1066,7 +1064,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -1110,7 +1108,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose =