From 7c39e319c6e45a2d023a15d926e4fa2597dec671 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 13 Dec 2023 13:57:38 +0900 Subject: [PATCH 1/5] use attention_lanelets Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 34 +++++++------------ .../src/scene_intersection.hpp | 6 ++-- .../src/util.cpp | 10 ++++-- .../src/util.hpp | 8 ++--- 4 files changed, 27 insertions(+), 31 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 0d483b501d1ee..47987ca05efab 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1070,15 +1070,6 @@ 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_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 +1115,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + // 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( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_); + 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}; + } + const double is_amber_or_red = (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); @@ -1376,13 +1376,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 +1389,8 @@ 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, 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..65a508243bb72 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1180,10 +1180,13 @@ bool checkStuckVehicleInIntersection( } 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) + [[maybe_unused]] const util::TargetObjects & target_objects, + [[maybe_unused]] const util::InterpolatedPathInfo & interpolated_path_info, + [[maybe_unused]] const lanelet::ConstLanelets & attention_lanelets, + [[maybe_unused]] const double stuck_vehicle_vel_thr, + [[maybe_unused]] const double yield_stuck_distance_thr, [[maybe_unused]] 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) { @@ -1215,6 +1218,7 @@ bool checkYieldStuckVehicleInIntersection( return true; } } + */ return false; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 8d0e673fc931e..2e8ef74dbc69a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -131,10 +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, - DebugData * debug_data); + const util::TargetObjects & target_objects, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, 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); From a861078d6c58d9774f42b49467a3fd8c49ce9e9c Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 15 Dec 2023 12:47:38 +0900 Subject: [PATCH 2/5] for left(right) direction , use right(left) boundary Signed-off-by: Mamoru Sobue --- .../README.md | 6 + .../config/intersection.param.yaml | 4 +- .../docs/yield-stuck.drawio.svg | 750 ++++++++++++++++++ .../src/debug.cpp | 8 + .../src/scene_intersection.cpp | 3 +- .../src/util.cpp | 163 +++- .../src/util.hpp | 5 +- .../src/util_type.hpp | 1 + 8 files changed, 905 insertions(+), 35 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index db26be11b1c9a..16434b662f1d1 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. + +![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 47987ca05efab..ef7e8e06dc9d5 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1390,7 +1390,8 @@ bool IntersectionModule::checkYieldStuckVehicle( } return util::checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, attention_lanelets, + 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/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 65a508243bb72..61aa376bb75eb 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 @@ -1179,46 +1180,147 @@ bool checkStuckVehicleInIntersection( return false; } -bool checkYieldStuckVehicleInIntersection( - [[maybe_unused]] const util::TargetObjects & target_objects, - [[maybe_unused]] const util::InterpolatedPathInfo & interpolated_path_info, - [[maybe_unused]] const lanelet::ConstLanelets & attention_lanelets, - [[maybe_unused]] const double stuck_vehicle_vel_thr, - [[maybe_unused]] const double yield_stuck_distance_thr, [[maybe_unused]] 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; } @@ -1531,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(Mamoru Sobue): 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 2e8ef74dbc69a..33e511d911b82 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -133,8 +133,9 @@ bool checkStuckVehicleInIntersection( bool checkYieldStuckVehicleInIntersection( const util::TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, const double stuck_vehicle_vel_thr, - const double yield_stuck_distance_thr, DebugData * debug_data); + 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( 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 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; From d288d089a31ed5c34e941deaff87c0c68f3226fb Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 18 Dec 2023 09:48:33 +0900 Subject: [PATCH 3/5] refactor stuck/yield_stuck for right turn Signed-off-by: Mamoru Sobue --- .../README.md | 2 +- .../src/scene_intersection.cpp | 58 ++++++++++++++----- 2 files changed, 45 insertions(+), 15 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 16434b662f1d1..63be544fabf50 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -186,7 +186,7 @@ If there is any object on the path inside the intersection and at the exit of th ## 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. +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 yieling against ego or the object is waiting before the crosswalk around the exit of the intersection. ![yield_stuck_detection](./docs/yield-stuck.drawio.svg) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index ef7e8e06dc9d5..e036fac13fd3b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1056,17 +1056,30 @@ 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}; + } } } @@ -1115,13 +1128,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - // 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( target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_); - 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 (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 = From 355e0d77c1cc2d527ccc792b557600c47023237a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 18 Dec 2023 13:38:36 +0900 Subject: [PATCH 4/5] fix spell Signed-off-by: Mamoru Sobue --- planning/behavior_velocity_intersection_module/README.md | 2 +- planning/behavior_velocity_intersection_module/src/util.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 63be544fabf50..58c2ce59edd48 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -186,7 +186,7 @@ If there is any object on the path inside the intersection and at the exit of th ## 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 yieling against ego or the object is waiting before the crosswalk around the exit of the intersection. +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) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 61aa376bb75eb..9623bf74f2532 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -299,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; } From e1895bbdfe9ac51464c08330716a14bace8f2e58 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 18 Dec 2023 14:40:39 +0900 Subject: [PATCH 5/5] fix Mamoru Sobue Signed-off-by: Mamoru Sobue --- planning/behavior_velocity_intersection_module/src/util.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 9623bf74f2532..e491d2ce7c5ce 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1633,7 +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(Mamoru Sobue): maybe this is opposite + // 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);