diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index bb80c140b95fb..30f4bc7b42a25 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -24,6 +24,12 @@ # 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. + yield_stuck: + turn_direction: + left: true + right: false + straight: false + distance_thr: 1.0 # [m] collision_detection: state_transit_margin_time: 1.0 @@ -42,9 +48,9 @@ use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: - distance_to_assigned_lanelet_start: 5.0 - duration: 2.0 - range: 30.0 # [m] + distance_to_assigned_lanelet_start: 10.0 + duration: 3.0 + range: 50.0 # [m] occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 9ff638cb61876..82f5b12966c12 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -238,6 +238,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3cca1f42115d1..93d67e786cff5 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -85,6 +85,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) 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.stuck_vehicle.yield_stuck_turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.left"); + ip.stuck_vehicle.yield_stuck_turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.right"); + ip.stuck_vehicle.yield_stuck_turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.straight"); + ip.stuck_vehicle.yield_stuck_distance_thr = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.distance_thr"); 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 ee541a061ce0e..76c2dc072dda9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -195,6 +195,21 @@ void prepareRTCByDecisionResult( return; } +template <> +void prepareRTCByDecisionResult( + const IntersectionModule::YieldStuckStop & result, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop"); + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; + *default_safety = false; + *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *occlusion_safety = true; + return; +} + template <> void prepareRTCByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & result, @@ -419,6 +434,38 @@ void reactRTCApprovalByDecisionResult( return; } +template <> +void reactRTCApprovalByDecisionResult( + const bool rtc_default_approved, const bool rtc_occlusion_approved, + const IntersectionModule::YieldStuckStop & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("reactRTCApprovalByDecisionResult"), + "YieldStuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, + rtc_occlusion_approved); + const auto closest_idx = decision_result.closest_idx; + if (!rtc_default_approved) { + // use default_rtc uuid for stuck vehicle detection + const auto stop_line_idx = decision_result.stuck_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->collision_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + return; +} + template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, @@ -780,6 +827,9 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult if (std::holds_alternative(decision_result)) { return "StuckStop"; } + if (std::holds_alternative(decision_result)) { + return "YieldStuckStop"; + } if (std::holds_alternative(decision_result)) { return "NonOccludedCollisionStop"; } @@ -985,6 +1035,23 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } + // yield stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const bool yield_stuck_detected = checkYieldStuckVehicle( + planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); + if (yield_stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); + 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::YieldStuckStop{closest_idx, stuck_stop_line_idx}; + } + } + // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { return IntersectionModule::Indecisive{"attention area is empty"}; @@ -1234,6 +1301,37 @@ bool IntersectionModule::checkStuckVehicle( &debug_data_); } +bool IntersectionModule::checkYieldStuckVehicle( + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, + const std::optional & first_attention_area) +{ + if (!first_attention_area) { + return false; + } + + const bool yield_stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.left) || + (turn_direction_ == "right" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.right) || + (turn_direction_ == "straight" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.straight); + }(); + if (!yield_stuck_detection_direction) { + return false; + } + + const auto & objects_ptr = planner_data->predicted_objects; + + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + return util::checkYieldStuckVehicleInIntersection( + objects_ptr, ego_poly, first_attention_area.value(), + planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + planner_param_.stuck_vehicle.yield_stuck_distance_thr, &debug_data_); +} + autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6ae734bd6e05b..a4804018d194b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -66,7 +66,8 @@ class IntersectionModule : public SceneModuleInterface bool left; bool right; bool straight; - } turn_direction; + }; + TurnDirection 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 @@ -77,6 +78,8 @@ class IntersectionModule : public SceneModuleInterface */ double timeout_private_area; bool enable_private_area_stuck_disregard; + double yield_stuck_distance_thr; + TurnDirection yield_stuck_turn_direction; } stuck_vehicle; struct CollisionDetection { @@ -147,6 +150,11 @@ class IntersectionModule : public SceneModuleInterface size_t stuck_stop_line_idx{0}; std::optional occlusion_stop_line_idx{std::nullopt}; }; + struct YieldStuckStop + { + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; + }; struct NonOccludedCollisionStop { size_t closest_idx{0}; @@ -206,6 +214,7 @@ class IntersectionModule : public SceneModuleInterface using DecisionResult = std::variant< Indecisive, // internal process error, or over the pass judge line StuckStop, // detected stuck vehicle + YieldStuckStop, // detected yield stuck vehicle NonOccludedCollisionStop, // detected collision while FOV is clear FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion PeekingTowardOcclusion, // peeking into occlusion while collision is not detected @@ -288,6 +297,11 @@ class IntersectionModule : public SceneModuleInterface const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets); + bool checkYieldStuckVehicle( + const std::shared_ptr & planner_data, + const util::PathLanelets & path_lanelets, + const std::optional & first_attention_area); + autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index ffc0b61880188..17df31ba3ffc9 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1108,6 +1108,45 @@ bool checkStuckVehicleInIntersection( return false; } +bool checkYieldStuckVehicleInIntersection( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, + const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data) +{ + const auto first_attention_area_2d = lanelet::utils::to2D(first_attention_area); + Polygon2d first_attention_area_poly; + for (const auto & p : first_attention_area_2d) { + first_attention_area_poly.outer().emplace_back(p.x(), p.y()); + } + + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + 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); + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; // not stop vehicle + } + + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + + // check if the object is too close to the ego path + if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) { + continue; + } + + // check if the footprint is in the stuck detect area + const bool is_in_stuck_area = bg::within(obj_footprint, first_attention_area_poly); + if (is_in_stuck_area && debug_data) { + debug_data->yield_stuck_targets.objects.push_back(object); + return true; + } + } + return false; +} + Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) { diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index a75545353fb7a..001aa63c7fe12 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -125,6 +125,12 @@ bool checkStuckVehicleInIntersection( const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, DebugData * debug_data); +bool checkYieldStuckVehicleInIntersection( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, + const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, + DebugData * debug_data); + Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 581f22904fd2b..11c8d7d8407b7 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -50,6 +50,7 @@ struct DebugData std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt};