From d1a77d59fc6b676e94bd9ef62a644c310b676c3f Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sun, 15 Oct 2023 13:50:03 +0900 Subject: [PATCH] feat(intersection): ignore decelerating vehicle on amber traffic light (#5304) * merge attention area lanelets topologically Signed-off-by: Mamoru Sobue * query stop line for each merged detection lanelet Signed-off-by: Mamoru Sobue * first conflicting/attention area Signed-off-by: Mamoru Sobue * ignore expectedToStopBeforeStopLine vehicle Signed-off-by: Mamoru Sobue * ignore expectedToStopBeforeStopLine vehicle Signed-off-by: Mamoru Sobue * fix Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 8 +- .../src/debug.cpp | 5 + .../src/manager.cpp | 8 +- .../src/scene_intersection.cpp | 314 ++++++++-------- .../src/scene_intersection.hpp | 26 +- .../src/util.cpp | 334 +++++++++++------- .../src/util.hpp | 15 +- .../src/util_type.hpp | 59 +++- 8 files changed, 455 insertions(+), 314 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 30f4bc7b42a25..1a5143678ddce 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -48,9 +48,11 @@ 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: 10.0 - duration: 3.0 - range: 50.0 # [m] + distance_to_assigned_lanelet_start: 5.0 + duration: 2.0 + object_dist_to_stopline: 10.0 # [m] + ignore_on_amber_traffic_light: + object_expected_deceleration: 2.0 # [m/ss] occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 82f5b12966c12..3d5874c3c89e8 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -233,6 +233,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 93d67e786cff5..22fa57f20e79f 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -129,8 +129,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & 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.range = getOrDeclareParameter( - node, ns + ".collision_detection.yield_on_green_traffic_light.range"); + 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 = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 76c2dc072dda9..b37b70f290ff6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1103,7 +1103,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); - const auto & attention_lanelets = intersection_lanelets.attention(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); @@ -1120,8 +1119,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); // If there are any vehicles on the attention area when ego entered the intersection on green // light, do pseudo collision detection because the vehicles are very slow and no collisions may @@ -1145,11 +1143,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } if (initial_green_light_observed_time_) { const auto now = clock_->now(); - const bool exist_close_vehicles = std::any_of( - target_objects.objects.begin(), target_objects.objects.end(), [&](const auto & object) { - return tier4_autoware_utils::calcDistance3d( - object.kinematics.initial_pose_with_covariance.pose, current_pose) < - planner_param_.collision_detection.yield_on_green_traffic_light.range; + const bool exist_close_vehicles = + std::any_of(target_objects.all.begin(), target_objects.all.end(), [&](const auto & object) { + return object.dist_to_stop_line.has_value() && + object.dist_to_stop_line.value() < + planner_param_.collision_detection.yield_on_green_traffic_light + .object_dist_to_stopline; }); if ( exist_close_vehicles && @@ -1168,7 +1167,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getDuration()); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, + *path, &target_objects, path_lanelets, closest_idx, std::min(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( @@ -1195,16 +1194,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector blocking_attention_objects; - std::copy_if( - target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(blocking_attention_objects), - [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { - return std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; - }); - debug_data_.blocking_attention_objects.objects = blocking_attention_objects; + const auto blocking_attention_objects = target_objects.parked_attention_objects; + for (const auto & blocking_attention_object : blocking_attention_objects) { + debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + } + // debug_data_.blocking_attention_objects.objects = blocking_attention_objects; const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) ? isOcclusionCleared( @@ -1332,18 +1326,17 @@ bool IntersectionModule::checkYieldStuckVehicle( 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, +util::TargetObjects IntersectionModule::generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; - const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - autoware_auto_perception_msgs::msg::PredictedObjects target_objects; + util::TargetObjects target_objects; target_objects.header = objects_ptr->header; + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stop_lines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); for (const auto & object : objects_ptr->objects) { // ignore non-vehicle type objects, such as pedestrian. if (!isTargetCollisionVehicleType(object)) { @@ -1352,49 +1345,72 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold); - if (is_in_adjacent_lanelets) { + planner_param_.common.attention_area_margin, false); + if (belong_adjacent_lanelet_id) { continue; } + const auto is_parked_vehicle = + std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + auto & container = is_parked_vehicle ? target_objects.parked_attention_objects + : target_objects.attention_objects; if (intersection_area) { const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); - if (is_in_intersection_area) { - target_objects.objects.push_back(object); - } else if (util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - attention_area_lanelets, planner_param_.common.attention_area_angle_thr, - planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { - target_objects.objects.push_back(object); + const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, planner_param_.common.attention_area_angle_thr, + planner_param_.common.consider_wrong_direction_vehicle, + planner_param_.common.attention_area_margin, is_parked_vehicle); + if (belong_attention_lanelet_id) { + const auto id = belong_adjacent_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); + } else if (bg::within(obj_poly, intersection_area_2d)) { + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = std::nullopt; + target_object.stop_line = std::nullopt; + container.push_back(target_object); } - } else if (util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - attention_area_lanelets, planner_param_.common.attention_area_angle_thr, + } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, + planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { + planner_param_.common.attention_area_margin, is_parked_vehicle); + belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before - target_objects.objects.push_back(object); + const auto id = belong_attention_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); } } + for (const auto & object : target_objects.attention_objects) { + target_objects.all.push_back(object); + } + for (const auto & object : target_objects.parked_attention_objects) { + target_objects.all.push_back(object); + } + for (auto & object : target_objects.all) { + object.calc_dist_to_stop_line(); + } return target_objects; } bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const util::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stop_line_candidate_idx, const double time_delay, - const util::TrafficPrioritizedLevel & traffic_prioritized_level) + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1408,17 +1424,16 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.use_upstream_velocity, planner_param_.collision_detection.minimum_upstream_velocity); const double passing_time = time_distance_array.back().first; - auto target_objects = objects; - util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); + util::cutPredictPathWithDuration(target_objects, clock_, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const auto & ego_lane = path_lanelets.ego_or_entry2exit; debug_data_.ego_lane = ego_lane.polygon3d(); - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - // check collision between predicted_path and ego_area + + // change TTC margin based on ego traffic light color const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { return std::make_pair( @@ -1434,8 +1449,33 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.not_prioritized.collision_start_margin_time, planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); + const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { + if (!target_object.dist_to_stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + if (dist_to_stop_line < 0) { + return false; + } + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + return dist_to_stop_line > braking_distance; + }; + + // check collision between predicted_path and ego_area bool collision_detected = false; - for (const auto & object : target_objects.objects) { + for (const auto & target_object : target_objects->all) { + const auto & object = target_object.object; + // If the vehicle is expected to stop before their stopline, ignore + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + expectedToStopBeforeStopLine(target_object)) { + debug_data_.amber_ignore_targets.objects.push_back(object); + continue; + } for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1532,9 +1572,8 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - blocking_attention_objects, + const std::vector & lane_divisions, + const std::vector & blocking_attention_objects, const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; @@ -1660,27 +1699,24 @@ bool IntersectionModule::isOcclusionCleared( // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded std::vector> blocking_polygons; for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object); + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); } - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - for (const auto & division : divisions) { - bool blocking_vehicle_found = false; - for (const auto & point_it : division) { - const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); - if (!valid) continue; - if (blocking_vehicle_found) { - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - continue; - } - if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { - blocking_vehicle_found = true; - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - } + for (const auto & division : lane_divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; } } } @@ -1753,84 +1789,78 @@ bool IntersectionModule::isOcclusionCleared( } } - auto findNearestPointToProjection = - [](lanelet::ConstLineString2d division, const Point2d & projection, const double dist_thresh) { - double min_dist = std::numeric_limits::infinity(); - auto nearest = division.end(); - for (auto it = division.begin(); it != division.end(); it++) { - const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); - if (dist < min_dist) { - min_dist = dist; - nearest = it; - } - if (dist < dist_thresh) { - break; - } + auto findNearestPointToProjection = []( + const lanelet::ConstLineString3d & division, + const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; } - return nearest; - }; + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; struct NearestOcclusionPoint { - int lane_id; int64 division_index; double dist; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; } nearest_occlusion_point; double min_dist = std::numeric_limits::infinity(); - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - const auto lane_id = lane_division.lane_id; - for (unsigned division_index = 0; division_index < divisions.size(); ++division_index) { - const auto & division = divisions.at(division_index); - LineString2d division_linestring; - auto division_point_it = division.begin(); - division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); - for (auto point_it = division.begin(); point_it != division.end(); point_it++) { - if ( - std::hypot( - point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < - 3.0 /* rough tick for computational cost */) { - continue; - } - division_linestring.emplace_back(point_it->x(), point_it->y()); - division_point_it = point_it; - } - - // find the intersection point of lane_line and path - std::vector intersection_points; - boost::geometry::intersection(division_linestring, path_linestring, intersection_points); - if (intersection_points.empty()) { + for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { + const auto & division = lane_divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { continue; } - const auto & projection_point = intersection_points.at(0); - const auto projection_it = - findNearestPointToProjection(division, projection_point, resolution); - if (projection_it == division.end()) { - continue; + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + // TODO(Mamoru Sobue): add handling for blocking vehicles + if (!valid) continue; + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; } - double acc_dist = 0.0; - auto acc_dist_it = projection_it; - for (auto point_it = projection_it; point_it != division.end(); point_it++) { - const double dist = - std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); - acc_dist += dist; - acc_dist_it = point_it; - const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); - // TODO(Mamoru Sobue): add handling for blocking vehicles - if (!valid) continue; - const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); - if (pixel == BLOCKED) { - break; - } - if (pixel == OCCLUDED) { - if (acc_dist < min_dist) { - min_dist = acc_dist; - nearest_occlusion_point = { - lane_id, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; - } + if (pixel == OCCLUDED) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; } } } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index a4804018d194b..beb13ef7bef7a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -109,8 +109,12 @@ class IntersectionModule : public SceneModuleInterface { double distance_to_assigned_lanelet_start; double duration; - double range; + double object_dist_to_stopline; } yield_on_green_traffic_light; + struct IgnoreOnAmberTrafficLight + { + double object_expected_deceleration; + } ignore_on_amber_traffic_light; } collision_detection; struct Occlusion { @@ -266,7 +270,8 @@ class IntersectionModule : public SceneModuleInterface // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_{std::nullopt}; + std::optional> occlusion_attention_divisions_{ + std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; @@ -302,17 +307,15 @@ class IntersectionModule : public SceneModuleInterface 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, + util::TargetObjects generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const util::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stop_line_candidate_idx, const double time_delay, - const util::TrafficPrioritizedLevel & traffic_prioritized_level); + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); bool isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, @@ -320,9 +323,8 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - parked_attention_objects, + const std::vector & lane_divisions, + const std::vector & blocking_attention_objects, const double occlusion_dist_thr); /* diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 17df31ba3ffc9..08c818c2e49ce 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -239,7 +239,8 @@ static std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } -static std::optional> +static std::optional> getFirstPointInsidePolygonsByFootprint( const std::vector & polygons, const InterpolatedPathInfo & interpolated_path_info, @@ -252,12 +253,11 @@ getFirstPointInsidePolygonsByFootprint( const auto & pose = path_ip.points.at(i).point.pose; const auto path_footprint = tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (const auto & polygon : polygons) { - const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); const bool is_in_polygon = bg::intersects(area_2d, path_footprint); if (is_in_polygon) { - return std::make_optional>( - i, polygon); + return std::make_optional>(i, j); } } } @@ -570,6 +570,101 @@ static std::vector getPolygon3dFromLanelets( return polys; } +static std::string getTurnDirection(lanelet::ConstLanelet lane) +{ + return lane.attributeOr("turn_direction", "else"); +} + +/** + * @param pair lanelets and the vector of original lanelets in topological order (not reversed as + *in generateDetectionLaneDivisions()) + **/ +static std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + const int n_node = lanelets.size(); + std::vector> adjacency(n_node); + for (int dst = 0; dst < n_node; ++dst) { + adjacency[dst].resize(n_node); + for (int src = 0; src < n_node; ++src) { + adjacency[dst][src] = false; + } + } + std::set lanelet_ids; + std::unordered_map id2ind; + std::unordered_map ind2id; + std::unordered_map id2lanelet; + int ind = 0; + for (const auto & lanelet : lanelets) { + lanelet_ids.insert(lanelet.id()); + const auto id = lanelet.id(); + id2ind[id] = ind; + ind2id[ind] = id; + id2lanelet[id] = lanelet; + ind++; + } + for (const auto & lanelet : lanelets) { + const auto & followings = routing_graph_ptr->following(lanelet); + const int dst = lanelet.id(); + for (const auto & following : followings) { + if (const int src = following.id(); lanelet_ids.find(src) != lanelet_ids.end()) { + adjacency[(id2ind[src])][(id2ind[dst])] = true; + } + } + } + // terminal node + std::map> branches; + auto has_no_previous = [&](const int node) { + for (int dst = 0; dst < n_node; dst++) { + if (adjacency[dst][node]) { + return false; + } + } + return true; + }; + for (int src = 0; src < n_node; src++) { + if (!has_no_previous(src)) { + continue; + } + branches[(ind2id[src])] = std::vector{}; + auto & branch = branches[(ind2id[src])]; + lanelet::Id node_iter = ind2id[src]; + while (true) { + const auto & destinations = adjacency[(id2ind[node_iter])]; + // NOTE: assuming detection lanelets have only one previous lanelet + const auto next = std::find(destinations.begin(), destinations.end(), true); + if (next == destinations.end()) { + branch.push_back(node_iter); + break; + } + branch.push_back(node_iter); + node_iter = ind2id[std::distance(destinations.begin(), next)]; + } + } + for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { + auto & branch = it->second; + std::reverse(branch.begin(), branch.end()); + } + lanelet::ConstLanelets merged; + std::vector originals; + for (const auto & [id, sub_ids] : branches) { + if (sub_ids.size() == 0) { + continue; + } + lanelet::ConstLanelets merge; + originals.push_back(lanelet::ConstLanelets({})); + auto & original = originals.back(); + for (const auto sub_id : sub_ids) { + merge.push_back(id2lanelet[sub_id]); + original.push_back(id2lanelet[sub_id]); + } + merged.push_back(lanelet::utils::combineLaneletsShape(merge)); + } + return {merged, originals}; +} + IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, @@ -707,14 +802,52 @@ IntersectionLanelets getObjectiveLanelets( } } } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const auto turn_direction = getTurnDirection(ll); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); IntersectionLanelets result; - result.attention_ = std::move(detection_and_preceding_lanelets); + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stop_line{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + break; + } + if (stop_line) break; + } + result.attention_stop_lines_.push_back(stop_line); + } result.attention_non_preceding_ = std::move(detection_lanelets); + // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + result.attention_non_preceding_stop_lines_.push_back(std::nullopt); + } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets); - // compoundPolygon3d + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched result.attention_area_ = getPolygon3dFromLanelets(result.attention_); result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); @@ -723,11 +856,6 @@ IntersectionLanelets getObjectiveLanelets( return result; } -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - bool isOverTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx) @@ -893,21 +1021,17 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); } -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, const double curvature_threshold, const double curvature_calculation_ds) { using lanelet::utils::getCenterlineWithOffset; - using lanelet::utils::to2D; // (0) remove left/right lanelet lanelet::ConstLanelets detection_lanelets; for (const auto & detection_lanelet : detection_lanelets_all) { - const auto turn_direction = getTurnDirection(detection_lanelet); - if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { - continue; - } + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet const auto fine_centerline = lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); const double highest_curvature = getHighestCurvature(fine_centerline); @@ -918,111 +1042,34 @@ std::vector generateDetectionLaneDivisions( } // (1) tsort detection_lanelets - // generate adjacency matrix - // if lanelet1 => lanelet2; then adjacency[lanelet2][lanelet1] = true - const int n_node = detection_lanelets.size(); - std::vector> adjacency(n_node); - for (int dst = 0; dst < n_node; ++dst) { - adjacency[dst].resize(n_node); - for (int src = 0; src < n_node; ++src) { - adjacency[dst][src] = false; - } - } - std::set detection_lanelet_ids; - std::unordered_map id2ind, ind2id; - std::unordered_map id2lanelet; - int ind = 0; - for (const auto & detection_lanelet : detection_lanelets) { - detection_lanelet_ids.insert(detection_lanelet.id()); - const int id = detection_lanelet.id(); - id2ind[id] = ind; - ind2id[ind] = id; - id2lanelet[id] = detection_lanelet; - ind++; - } - for (const auto & detection_lanelet : detection_lanelets) { - const auto & followings = routing_graph_ptr->following(detection_lanelet); - const int dst = detection_lanelet.id(); - for (const auto & following : followings) { - if (const int src = following.id(); - detection_lanelet_ids.find(src) != detection_lanelet_ids.end()) { - adjacency[(id2ind[src])][(id2ind[dst])] = true; - } - } - } - // terminal node - std::map> branches; - auto has_no_previous = [&](const int node) { - for (int dst = 0; dst < n_node; dst++) { - if (adjacency[dst][node]) { - return false; - } - } - return true; - }; - for (int src = 0; src < n_node; src++) { - if (!has_no_previous(src)) { - continue; - } - branches[(ind2id[src])] = std::vector{}; - auto & branch = branches[(ind2id[src])]; - int node_iter = ind2id[src]; - while (true) { - const auto & destinations = adjacency[(id2ind[node_iter])]; - // NOTE: assuming detection lanelets have only one previous lanelet - const auto next = std::find(destinations.begin(), destinations.end(), true); - if (next == destinations.end()) { - branch.push_back(node_iter); - break; - } - branch.push_back(node_iter); - node_iter = ind2id[std::distance(destinations.begin(), next)]; - } - } - for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { - auto & branch = it->second; - std::reverse(branch.begin(), branch.end()); - } + const auto [merged_detection_lanelets, originals] = + mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); // (2) merge each branch to one lanelet // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::unordered_map> merged_branches; - for (const auto & [src, branch] : branches) { - lanelet::Points3d lefts; - lanelet::Points3d rights; + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); double area = 0; - for (const auto & lane_id : branch) { - const auto lane = id2lanelet[lane_id]; - for (const auto & left_point : lane.leftBound()) { - lefts.push_back(lanelet::Point3d(left_point)); - } - for (const auto & right_point : lane.rightBound()) { - rights.push_back(lanelet::Point3d(right_point)); - } - area += bg::area(lane.polygon2d().basicPolygon()); + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); } - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, lefts).invert(); - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, rights).invert(); - lanelet::Lanelet merged = lanelet::Lanelet(lanelet::InvalId, left, right); - merged_branches[src] = std::make_pair(merged, area); + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); } // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [last_lane_id, branch] : merged_branches) { - DiscretizedLane detection_division; - detection_division.lane_id = last_lane_id; - const auto detection_lanelet = branch.first; - const double area = branch.second; - const double length = bg::length(detection_lanelet.centerline()); + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); const double width = area / length; - auto & divisions = detection_division.divisions; for (int i = 0; i < static_cast(width / resolution); ++i) { const double offset = resolution * i - width / 2; - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, offset, resolution))); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); } - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, width / 2, resolution))); - detection_divisions.push_back(detection_division); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); } return detection_divisions; } @@ -1186,13 +1233,13 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( return polygon; } -bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, - const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, - const bool consider_wrong_direction_vehicle, const double dist_margin, - const double parked_vehicle_speed_threshold) +std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double dist_margin, const bool is_parked_vehicle) { - for (const auto & ll : target_lanelets) { + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { continue; } @@ -1201,39 +1248,38 @@ bool checkAngleForTargetLanelets( const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } } else { if (std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is // positive if ( - std::fabs(longitudinal_velocity) < parked_vehicle_speed_threshold && - (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return true; + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); } } } - return false; + return std::nullopt; } void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr) + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) { const rclcpp::Time current_time = clock->now(); - for (auto & object : objects_ptr->objects) { // each objects - for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + for (auto & target_object : target_objects->all) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths const auto origin_path = predicted_path; predicted_path.path.clear(); for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points const auto & predicted_pose = origin_path.path.at(k); const auto predicted_time = - rclcpp::Time(objects_ptr->header.stamp) + + rclcpp::Time(target_objects->header.stamp) + rclcpp::Duration(origin_path.time_step) * static_cast(k); if ((predicted_time - current_time).seconds() < time_thr) { predicted_path.path.push_back(predicted_pose); @@ -1342,14 +1388,16 @@ void IntersectionLanelets::update( auto first = getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); if (first) { - first_conflicting_area_ = first.value().second; + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); } } if (!first_attention_area_) { - auto first = - getFirstPointInsidePolygonsByFootprint(attention_area_, interpolated_path_info, footprint); + auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint); if (first) { - first_attention_area_ = first.value().second; + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); } } } @@ -1435,5 +1483,23 @@ std::optional generatePathLanelets( return path_lanelets; } +void TargetObject::calc_dist_to_stop_line() +{ + if (!attention_lanelet || !stop_line) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stop_line_val = stop_line.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stop_line_val.front().x() + stop_line_val.back().x()) / 2.0; + stopline_center.position.y = (stop_line_val.front().y() + stop_line_val.back().y()) / 2.0; + stopline_center.position.z = (stop_line_val.front().z() + stop_line_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stop_line = (stopline_arc_coords.length - object_arc_coords.length); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 001aa63c7fe12..ef658a25fce55 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -107,7 +107,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); TrafficPrioritizedLevel getTrafficPrioritizedLevel( lanelet::ConstLanelet lane, const std::map & tl_infos); -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, const double curvature_threshold, const double curvature_calculation_ds); @@ -134,15 +134,14 @@ bool checkYieldStuckVehicleInIntersection( Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); -bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, - const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, - const bool consider_wrong_direction_vehicle, const double dist_margin, - const double parked_vehicle_speed_threshold); +std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double dist_margin, const bool is_parked_vehicle); void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr); + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, + const double time_thr); TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 11c8d7d8407b7..3f09b54f88be4 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -49,6 +49,7 @@ struct DebugData std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; @@ -77,6 +78,10 @@ struct IntersectionLanelets { return is_prioritized_ ? attention_non_preceding_ : attention_; } + const std::vector> & attention_stop_lines() const + { + return is_prioritized_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_; + } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } const lanelet::ConstLanelets & occlusion_attention() const @@ -96,38 +101,48 @@ struct IntersectionLanelets { return occlusion_attention_area_; } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } const std::optional & first_conflicting_area() const { return first_conflicting_area_; } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } const std::optional & first_attention_area() const { return first_attention_area_; } - lanelet::ConstLanelets attention_; + lanelet::ConstLanelets attention_; // topologically merged lanelets + std::vector> + attention_stop_lines_; // the stop lines for each attention_ lanelets lanelet::ConstLanelets attention_non_preceding_; + std::vector> + attention_non_preceding_stop_lines_; // the stop lines for each attention_non_preceding_ + // lanelets lanelet::ConstLanelets conflicting_; lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // for occlusion detection - std::vector attention_area_; + lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets + std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets + std::vector attention_area_; // topologically merged lanelets std::vector attention_non_preceding_area_; std::vector conflicting_area_; std::vector adjacent_area_; - std::vector occlusion_attention_area_; + std::vector + occlusion_attention_area_; // topologically merged lanelets // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; bool is_prioritized_ = false; - std::optional first_conflicting_area_ = std::nullopt; - std::optional first_attention_area_ = std::nullopt; -}; - -struct DiscretizedLane -{ - int lane_id{0}; - // discrete fine lines from left to right - std::vector divisions{}; }; struct IntersectionStopLines @@ -161,6 +176,24 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the path }; +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stop_line{std::nullopt}; + std::optional dist_to_stop_line{std::nullopt}; + void calc_dist_to_stop_line(); +}; + +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all; // TODO(Mamoru Sobue): avoid copy +}; + enum class TrafficPrioritizedLevel { // The target lane's traffic signal is red or the ego's traffic signal has an arrow. FULLY_PRIORITIZED = 0,