diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index ccd3612203af5..e210d2fb21b88 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -19,6 +19,7 @@ # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area + enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. collision_detection: state_transit_margin_time: 1.0 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 6f47dfbbe31fb..3c96a171516bd 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -77,6 +77,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) */ ip.stuck_vehicle.timeout_private_area = getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); + ip.stuck_vehicle.enable_private_area_stuck_disregard = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 10f1ed73bf435..b30a57f9e8dad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -96,10 +96,6 @@ IntersectionModule::IntersectionModule( occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); occlusion_stop_state_machine_.setState(StateMachine::State::GO); } - { - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); - } { temporal_stop_before_attention_state_machine_.setMarginTime( planner_param_.occlusion.before_creep_stop_time); @@ -918,16 +914,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); if (stuck_detected && stuck_stop_line_idx_opt) { auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - const bool stopped_at_stuck_line = stoppedForDuration( - stuck_stop_line_idx, planner_param_.stuck_vehicle.timeout_private_area, - stuck_private_area_timeout_); - const bool timeout = (is_private_area_ && stopped_at_stuck_line); - if (!timeout) { + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { if ( default_stop_line_idx_opt && fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { stuck_stop_line_idx = default_stop_line_idx_opt.value(); } + } else { return IntersectionModule::StuckStop{ closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index b3a8e69b70b6a..f1721e5ac5b4f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -71,6 +71,7 @@ class IntersectionModule : public SceneModuleInterface bool enable_front_car_decel_prediction; //! flag for using above feature */ double timeout_private_area; + bool enable_private_area_stuck_disregard; } stuck_vehicle; struct CollisionDetection { @@ -256,7 +257,6 @@ class IntersectionModule : public SceneModuleInterface // for stuck vehicle detection const bool is_private_area_; - StateMachine stuck_private_area_timeout_; // for RTC const UUID occlusion_uuid_;