From aec283e201a3588d2fc4cf240d5fe0fc36678b27 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 11 Oct 2023 17:29:04 +0900 Subject: [PATCH] fix(intersection): prevent unnecessary stop inside intersection without traffic light when occlusion feature is off Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 2 +- planning/behavior_velocity_intersection_module/src/util.cpp | 6 +++--- planning/behavior_velocity_intersection_module/src/util.hpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 76c2dc072dda9..1ccb1c2856a9d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -978,7 +978,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, - peeking_offset, path); + peeking_offset, planner_param_.occlusion.enable, path); if (!intersection_stop_lines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 17df31ba3ffc9..7b8e0feac262b 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -269,7 +269,7 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double peeking_offset, const double enable_occlusion, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; @@ -419,8 +419,8 @@ std::optional generateIntersectionStopLines( intersection_stop_lines_temp.default_stop_line; } if ( - intersection_stop_lines_temp.occlusion_peeking_stop_line > - intersection_stop_lines_temp.pass_judge_line) { + enable_occlusion && intersection_stop_lines_temp.occlusion_peeking_stop_line > + intersection_stop_lines_temp.pass_judge_line) { intersection_stop_lines_temp.pass_judge_line = intersection_stop_lines_temp.occlusion_peeking_stop_line; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 001aa63c7fe12..d2580ea03e9e0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -70,7 +70,7 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double peeking_offset, const double enable_occlusion, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional getFirstPointInsidePolygon(