diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index e934ad1d0dbfc..5ff420edc1c4d 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -15,6 +15,15 @@ enable_pass_judge_before_default_stopline: false stuck_vehicle: + target_type: + car: true + bus: true + truck: true + trailer: true + motorcycle: false + bicycle: false + unknown: false + turn_direction: left: true right: true @@ -27,6 +36,14 @@ disable_against_private_lane: true yield_stuck: + target_type: + car: true + bus: true + truck: true + trailer: true + motorcycle: false + bicycle: false + unknown: false turn_direction: left: true right: true @@ -37,6 +54,14 @@ consider_wrong_direction_vehicle: false collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 + target_type: + car: true + bus: true + truck: true + trailer: true + motorcycle: true + bicycle: true + unknown: false velocity_profile: use_upstream: true minimum_upstream_velocity: 0.01 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 0b4aabaa938f0..f82bbb0fbd5e6 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -42,141 +42,256 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) { const std::string ns(getModuleName()); auto & ip = intersection_param_; - ip.common.attention_area_length = - getOrDeclareParameter(node, ns + ".common.attention_area_length"); - ip.common.attention_area_margin = - getOrDeclareParameter(node, ns + ".common.attention_area_margin"); - ip.common.attention_area_angle_threshold = - getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); - ip.common.use_intersection_area = - getOrDeclareParameter(node, ns + ".common.use_intersection_area"); - ip.common.default_stopline_margin = - getOrDeclareParameter(node, ns + ".common.default_stopline_margin"); - ip.common.stopline_overshoot_margin = - getOrDeclareParameter(node, ns + ".common.stopline_overshoot_margin"); - ip.common.path_interpolation_ds = - getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); - ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); - ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); - ip.common.delay_response_time = - getOrDeclareParameter(node, ns + ".common.delay_response_time"); - ip.common.enable_pass_judge_before_default_stopline = - getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); - - 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 = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); - ip.stuck_vehicle.stuck_vehicle_velocity_threshold = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); - ip.stuck_vehicle.disable_against_private_lane = - getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); - - ip.yield_stuck.turn_direction.left = - getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); - ip.yield_stuck.turn_direction.right = - getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.right"); - ip.yield_stuck.turn_direction.straight = - getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.straight"); - ip.yield_stuck.distance_threshold = - getOrDeclareParameter(node, ns + ".yield_stuck.distance_threshold"); - - ip.collision_detection.consider_wrong_direction_vehicle = - getOrDeclareParameter(node, ns + ".collision_detection.consider_wrong_direction_vehicle"); - ip.collision_detection.collision_detection_hold_time = - getOrDeclareParameter(node, ns + ".collision_detection.collision_detection_hold_time"); - ip.collision_detection.min_predicted_path_confidence = - getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); - ip.collision_detection.velocity_profile.use_upstream = - getOrDeclareParameter(node, ns + ".collision_detection.velocity_profile.use_upstream"); - ip.collision_detection.velocity_profile.minimum_upstream_velocity = getOrDeclareParameter( - node, ns + ".collision_detection.velocity_profile.minimum_upstream_velocity"); - ip.collision_detection.velocity_profile.default_velocity = getOrDeclareParameter( - node, ns + ".collision_detection.velocity_profile.default_velocity"); - ip.collision_detection.velocity_profile.minimum_default_velocity = getOrDeclareParameter( - node, ns + ".collision_detection.velocity_profile.minimum_default_velocity"); - ip.collision_detection.fully_prioritized.collision_start_margin_time = - getOrDeclareParameter( - node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); - ip.collision_detection.fully_prioritized.collision_end_margin_time = - getOrDeclareParameter( - node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time"); - ip.collision_detection.partially_prioritized.collision_start_margin_time = - getOrDeclareParameter( - node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time"); - ip.collision_detection.partially_prioritized.collision_end_margin_time = - getOrDeclareParameter( - node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time"); - ip.collision_detection.not_prioritized.collision_start_margin_time = - getOrDeclareParameter( - node, ns + ".collision_detection.not_prioritized.collision_start_margin_time"); - ip.collision_detection.not_prioritized.collision_end_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); - ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = - getOrDeclareParameter( - node, - ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); - ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( - node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); - ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline = - getOrDeclareParameter( - node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline"); - ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car = - getOrDeclareParameter( - node, - ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car"); - ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike = - getOrDeclareParameter( - node, - ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike"); - ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = - getOrDeclareParameter( - node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); - ip.collision_detection.avoid_collision_by_acceleration - .object_time_margin_to_collision_point = getOrDeclareParameter( - node, - ns + - ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point"); - - ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); - ip.occlusion.occlusion_attention_area_length = - getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); - ip.occlusion.free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); - ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); - ip.occlusion.denoise_kernel = - getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); - ip.occlusion.attention_lane_crop_curvature_threshold = - getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); - ip.occlusion.attention_lane_curvature_calculation_ds = - getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); - ip.occlusion.creep_during_peeking.enable = - getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.enable"); - ip.occlusion.creep_during_peeking.creep_velocity = - getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); - ip.occlusion.peeking_offset = - getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); - ip.occlusion.occlusion_required_clearance_distance = - getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); - ip.occlusion.possible_object_bbox = - getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); - ip.occlusion.ignore_parked_vehicle_speed_threshold = - getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); - ip.occlusion.occlusion_detection_hold_time = - getOrDeclareParameter(node, ns + ".occlusion.occlusion_detection_hold_time"); - ip.occlusion.temporal_stop_time_before_peeking = - getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_time_before_peeking"); - ip.occlusion.temporal_stop_before_attention_area = - getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); - ip.occlusion.creep_velocity_without_traffic_light = - getOrDeclareParameter(node, ns + ".occlusion.creep_velocity_without_traffic_light"); - ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( - node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + + // common + { + ip.common.attention_area_length = + getOrDeclareParameter(node, ns + ".common.attention_area_length"); + ip.common.attention_area_margin = + getOrDeclareParameter(node, ns + ".common.attention_area_margin"); + ip.common.attention_area_angle_threshold = + getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); + ip.common.use_intersection_area = + getOrDeclareParameter(node, ns + ".common.use_intersection_area"); + ip.common.default_stopline_margin = + getOrDeclareParameter(node, ns + ".common.default_stopline_margin"); + ip.common.stopline_overshoot_margin = + getOrDeclareParameter(node, ns + ".common.stopline_overshoot_margin"); + ip.common.path_interpolation_ds = + getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); + ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); + ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); + ip.common.delay_response_time = + getOrDeclareParameter(node, ns + ".common.delay_response_time"); + ip.common.enable_pass_judge_before_default_stopline = + getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); + } + + // stuck + { + // target_type + { + ip.stuck_vehicle.target_type.car = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.car"); + ip.stuck_vehicle.target_type.bus = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bus"); + ip.stuck_vehicle.target_type.truck = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.truck"); + ip.stuck_vehicle.target_type.trailer = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.trailer"); + ip.stuck_vehicle.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.motorcycle"); + ip.stuck_vehicle.target_type.bicycle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bicycle"); + ip.stuck_vehicle.target_type.unknown = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.unknown"); + } + + // turn_direction + { + 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 = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); + ip.stuck_vehicle.stuck_vehicle_velocity_threshold = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); + ip.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); + } + + // yield_stuck + { + // target_type + { + ip.yield_stuck.target_type.car = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.car"); + ip.yield_stuck.target_type.bus = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bus"); + ip.yield_stuck.target_type.truck = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.truck"); + ip.yield_stuck.target_type.trailer = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.trailer"); + ip.yield_stuck.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.motorcycle"); + ip.yield_stuck.target_type.bicycle = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bicycle"); + ip.yield_stuck.target_type.unknown = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.unknown"); + } + + // turn_direction + { + ip.yield_stuck.turn_direction.left = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); + ip.yield_stuck.turn_direction.right = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.right"); + ip.yield_stuck.turn_direction.straight = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.straight"); + } + + ip.yield_stuck.distance_threshold = + getOrDeclareParameter(node, ns + ".yield_stuck.distance_threshold"); + } + + // collision_detection + { + ip.collision_detection.consider_wrong_direction_vehicle = getOrDeclareParameter( + node, ns + ".collision_detection.consider_wrong_direction_vehicle"); + ip.collision_detection.collision_detection_hold_time = getOrDeclareParameter( + node, ns + ".collision_detection.collision_detection_hold_time"); + ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter( + node, ns + ".collision_detection.min_predicted_path_confidence"); + + // target_type + { + ip.collision_detection.target_type.car = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.car"); + ip.collision_detection.target_type.bus = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.bus"); + ip.collision_detection.target_type.truck = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.truck"); + ip.collision_detection.target_type.trailer = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.trailer"); + ip.collision_detection.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.motorcycle"); + ip.collision_detection.target_type.bicycle = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.bicycle"); + ip.collision_detection.target_type.unknown = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.unknown"); + } + + // velocity_profile + { + ip.collision_detection.velocity_profile.use_upstream = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.use_upstream"); + ip.collision_detection.velocity_profile.minimum_upstream_velocity = + getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_upstream_velocity"); + ip.collision_detection.velocity_profile.default_velocity = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.default_velocity"); + ip.collision_detection.velocity_profile.minimum_default_velocity = + getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_default_velocity"); + } + + // fully_prioritized + { + ip.collision_detection.fully_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); + ip.collision_detection.fully_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time"); + } + + // partially_prioritized + { + ip.collision_detection.partially_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time"); + ip.collision_detection.partially_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time"); + } + + // not_prioritized + { + ip.collision_detection.not_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_start_margin_time"); + ip.collision_detection.not_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); + } + + // yield_on_green_traffic_light + { + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline = + getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline"); + } + + // ignore_on_amber_traffic_light, ignore_on_red_traffic_light + { + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car = + getOrDeclareParameter( + node, + ns + + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car"); + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike = + getOrDeclareParameter( + node, + ns + + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike"); + ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + } + + ip.collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point = + getOrDeclareParameter( + node, ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_" + "collision_point"); + } + + // occlusion + { + ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + ip.occlusion.occlusion_attention_area_length = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); + ip.occlusion.free_space_max = + getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + ip.occlusion.denoise_kernel = + getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); + ip.occlusion.attention_lane_crop_curvature_threshold = getOrDeclareParameter( + node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = getOrDeclareParameter( + node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); + + // creep_during_peeking + { + ip.occlusion.creep_during_peeking.enable = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.enable"); + ip.occlusion.creep_during_peeking.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); + } + + ip.occlusion.peeking_offset = + getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); + ip.occlusion.possible_object_bbox = + getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); + ip.occlusion.ignore_parked_vehicle_speed_threshold = + getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + ip.occlusion.occlusion_detection_hold_time = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_detection_hold_time"); + ip.occlusion.temporal_stop_time_before_peeking = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_time_before_peeking"); + ip.occlusion.temporal_stop_before_attention_area = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.creep_velocity_without_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.creep_velocity_without_traffic_light"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( + node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + } ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 9da765dfadc8a..c9a10cc8ba5b9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -74,8 +74,20 @@ class IntersectionModule : public SceneModuleInterface bool straight; }; + struct TargetType + { + bool car; + bool bus; + bool truck; + bool trailer; + bool motorcycle; + bool bicycle; + bool unknown; + }; + struct StuckVehicle { + TargetType target_type; TurnDirection turn_direction; bool use_stuck_stopline; double stuck_vehicle_detect_dist; @@ -85,6 +97,7 @@ class IntersectionModule : public SceneModuleInterface struct YieldStuck { + TargetType target_type; TurnDirection turn_direction; double distance_threshold; } yield_stuck; @@ -94,6 +107,7 @@ class IntersectionModule : public SceneModuleInterface bool consider_wrong_direction_vehicle; double collision_detection_hold_time; double min_predicted_path_confidence; + TargetType target_type; struct VelocityProfile { bool use_upstream; @@ -616,6 +630,9 @@ class IntersectionModule : public SceneModuleInterface bool isTargetStuckVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + bool isTargetYieldStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + /** * @brief check stuck */ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index ae6043f2658c3..0eff4cea63581 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -37,19 +37,29 @@ namespace bg = boost::geometry; bool IntersectionModule::isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const { + const auto label = object.classification.at(0).label; + const auto & p = planner_param_.collision_detection.target_type; + + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + return true; + } if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) { + label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 389c73d651f1e..a52c96567e928 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -170,17 +170,60 @@ std::optional IntersectionModule::isStuckStatus( bool IntersectionModule::isTargetStuckVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const { + const auto label = object.classification.at(0).label; + const auto & p = planner_param_.stuck_vehicle.target_type; + + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + return true; + } + if ( + label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + return true; + } + return false; +} + +bool IntersectionModule::isTargetYieldStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + const auto label = object.classification.at(0).label; + const auto & p = planner_param_.yield_stuck.target_type; + + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + return true; + } if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + return true; + } + if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; @@ -357,6 +400,9 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( debug_data_.yield_stuck_detect_area = util::getPolygon3dFromLanelets(yield_stuck_detect_lanelets); for (const auto & object_info : object_info_manager_.attentionObjects()) { const auto & object = object_info->predicted_object(); + if (!isTargetYieldStuckVehicleType(object)) { + continue; + } const auto obj_v_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y);