diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 30fefcaeee035..bb80c140b95fb 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -13,6 +13,10 @@ path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false stuck_vehicle: + turn_direction: + left: true + right: true + straight: true use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 4137090a5b6ae..3cca1f42115d1 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -63,6 +63,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.common.consider_wrong_direction_vehicle = getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); + ip.stuck_vehicle.turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); + ip.stuck_vehicle.turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.right"); + ip.stuck_vehicle.turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.straight"); ip.stuck_vehicle.use_stuck_stopline = getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 26bc8210b3bad..ee541a061ce0e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1213,8 +1213,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( bool IntersectionModule::checkStuckVehicle( const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { - // NOTE: No need to stop for stuck vehicle when the ego will turn left. - if (turn_direction_ == std::string("left")) { + const bool stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); + }(); + if (!stuck_detection_direction) { return false; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index de7d97a50c133..6ae734bd6e05b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -61,6 +61,12 @@ class IntersectionModule : public SceneModuleInterface } common; struct StuckVehicle { + struct TurnDirection + { + bool left; + bool right; + bool straight; + } turn_direction; bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped