diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index db26be11b1c9a..58c2ce59edd48 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -184,6 +184,12 @@ If there is any object on the path inside the intersection and at the exit of th ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) +## Yield stuck vehicle detection + +If there is any stopped object on the attention lanelet between the intersection point with ego path and the position which is `yield_stuck.distance_threshold` before that position, the object is regarded as yielding to ego vehicle. In this case ego is given the right-of-way by the yielding object but this module inserts stopline to prevent entry into the intersection. This scene happens when the object is yielding against ego or the object is waiting before the crosswalk around the exit of the intersection. + +![yield_stuck_detection](./docs/yield-stuck.drawio.svg) + ## Collision detection The following process is performed for the targets objects to determine whether ego can pass the intersection safely. If it is judged that ego cannot pass the intersection with enough margin, this module inserts a stopline on the path. diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 1e6ce843de528..997addd48d7f8 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -29,9 +29,9 @@ yield_stuck: turn_direction: left: true - right: false + right: true straight: false - distance_threshold: 1.0 + distance_threshold: 5.0 collision_detection: consider_wrong_direction_vehicle: false diff --git a/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg b/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg new file mode 100644 index 0000000000000..e0078540ba407 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg @@ -0,0 +1,750 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + w/ traffic light, right +
+
+ (Left-hand traffic) + + + +
+ + +
+
+
+ + w/ traffic light, right... + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention lane +
+
+
+ + attention lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + T-shape junction w/o traffic light +
+
+ Right-hand traffic +
+ +
+
+
+ + T-shape junction w/o traffic light... + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + + + + + + + +
+
+
+ + yield stuck detect area + +
+
+
+ + yield stuck detect area + + + + + + + + + + + + + + + +
+
+
+ + yield stuck detect area + +
+
+
+ + yield stuck detect area + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 8d9beb34d05ee..83e218185a5ad 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -196,6 +196,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.yield_stuck_detect_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, + 0.34509, 0.6588235), + &debug_marker_array); + } + if (debug_data_.ego_lane) { appendMarkerArray( createLaneletPolygonsMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 0d483b501d1ee..e036fac13fd3b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1056,29 +1056,33 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected && stuck_stopline_idx_opt) { - auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); + if (stuck_detected) { if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { - if ( - default_stopline_idx_opt && - fromEgoDist(stuck_stopline_idx) < -planner_param_.common.stopline_overshoot_margin) { - stuck_stopline_idx = default_stopline_idx_opt.value(); - } } else { - return IntersectionModule::StuckStop{ - closest_idx, stuck_stopline_idx, occlusion_peeking_stopline_idx_opt}; + std::optional stopline_idx = std::nullopt; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = default_stopline_idx_opt.value(); + } else if ( + first_attention_stopline_idx_opt && + fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return IntersectionModule::StuckStop{ + closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + } } } - // 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_stopline_idx_opt) { - const auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); - return IntersectionModule::YieldStuckStop{closest_idx, stuck_stopline_idx}; - } - // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { return IntersectionModule::Indecisive{"attention area is empty"}; @@ -1124,6 +1128,32 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + const bool yield_stuck_detected = checkYieldStuckVehicle( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_); + if (yield_stuck_detected) { + std::optional stopline_idx = std::nullopt; + const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; + const bool is_before_first_attention_stopline = + fromEgoDist(first_attention_stopline_idx) >= 0.0; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (is_before_default_stopline) { + stopline_idx = default_stopline_idx; + } else if (is_before_first_attention_stopline) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()}; + } + } + const double is_amber_or_red = (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); @@ -1376,13 +1406,10 @@ bool IntersectionModule::checkStuckVehicle( } bool IntersectionModule::checkYieldStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, - const std::optional & first_attention_area) + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets) { - if (!first_attention_area) { - return false; - } - const bool yield_stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || @@ -1392,13 +1419,9 @@ bool IntersectionModule::checkYieldStuckVehicle( 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(), + target_objects, interpolated_path_info, attention_lanelets, turn_direction_, + planner_data_->vehicle_info_.vehicle_width_m, planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, planner_param_.yield_stuck.distance_threshold, &debug_data_); } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 4c33c0960afc3..7366bdc1bc0e6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -336,9 +336,9 @@ class IntersectionModule : public SceneModuleInterface const util::PathLanelets & path_lanelets); bool checkYieldStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets, - const std::optional & first_attention_area); + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets); util::TargetObjects generateTargetObjects( const util::IntersectionLanelets & intersection_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 1c7e366347fec..e491d2ce7c5ce 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -298,7 +299,7 @@ std::optional generateIntersectionStopLines( const auto path_footprint = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // TODO(Mamoru Sobue): maybe consideration of braking dist is necessary + // NOTE: maybe consideration of braking dist is necessary first_footprint_attention_centerline_ip_opt = i; break; } @@ -1179,40 +1180,145 @@ 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) +static lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) { - 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 + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; } - 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 + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; } + accumulated_length += length; + } - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} - // check if the object is too close to the ego path - if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) { +static lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +bool checkYieldStuckVehicleInIntersection( + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, + const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, + DebugData * debug_data) +{ + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { continue; } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); + } + debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); + for (const auto & object : target_objects.all_attention_objects) { + const auto obj_v_norm = std::hypot( + object.object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.object.kinematics.initial_twist_with_covariance.twist.linear.y); - // 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; + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; + } + for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + if (is_in_lanelet) { + debug_data->yield_stuck_targets.objects.push_back(object.object); + return true; + } } } return false; @@ -1527,6 +1633,7 @@ lanelet::ConstLanelet generatePathLanelet( const double yaw = tf2::getYaw(p.orientation); const double x = p.position.x; const double y = p.position.y; + // NOTE: maybe this is opposite const double left_x = x + width / 2 * std::sin(yaw); const double left_y = y - width / 2 * std::cos(yaw); const double right_x = x - width / 2 * std::sin(yaw); diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 8d0e673fc931e..33e511d911b82 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -131,9 +131,10 @@ bool checkStuckVehicleInIntersection( 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, + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, + const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data); Polygon2d generateStuckVehicleDetectAreaPolygon( diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 3c7ba3041b0bd..d05031bf19436 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -45,6 +45,7 @@ struct DebugData std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;