diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index c643afa989b98..8b993db11b4d9 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,6 +12,8 @@ use_intersection_area: false path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false + disable_pass_judge_sudden_stop: false + enable_peeking_pass_judge: false stuck_vehicle: turn_direction: left: true diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 22fa57f20e79f..774ae760e6166 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -62,6 +62,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); ip.common.consider_wrong_direction_vehicle = getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); + ip.common.disable_pass_judge_sudden_stop = + getOrDeclareParameter(node, ns + ".common.disable_pass_judge_sudden_stop"); + ip.common.enable_peeking_pass_judge = + getOrDeclareParameter(node, ns + ".common.enable_peeking_pass_judge"); ip.stuck_vehicle.turn_direction.left = getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b37b70f290ff6..334f19db9d380 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); + planner_param_.common.enable_peeking_pass_judge, peeking_offset, path); if (!intersection_stop_lines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } @@ -1085,7 +1085,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); // do nothing } else if ( - (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || + (was_safe && + (is_over_default_stop_line || planner_param_.common.disable_pass_judge_sudden_stop) && + is_over_pass_judge_line && is_go_out_) || is_permanent_go_) { // is_go_out_: previous RTC approval // activated_: current RTC approval diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index dbc3b27f7b4ba..dde94a4e9c3be 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -58,6 +58,8 @@ class IntersectionModule : public SceneModuleInterface double stop_overshoot_margin; //! overshoot margin for stuck, collision detection bool use_intersection_area; bool consider_wrong_direction_vehicle; + bool disable_pass_judge_sudden_stop; + bool enable_peeking_pass_judge; double path_interpolation_ds; } common; struct StuckVehicle diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 797b415b62374..67e5d552771a0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -266,7 +266,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 bool enable_peeking_pass_judge, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; @@ -416,8 +416,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_peeking_pass_judge && (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 ef658a25fce55..5185fdf3477e4 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 bool enable_peeking_pass_judge, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional getFirstPointInsidePolygon(