From aacad9fc578904c4526c44e8711cd68eb9adb6a6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 25 Sep 2023 23:30:46 +0900 Subject: [PATCH] feat(intersection): modify occlusion distance calculation and publish projection line (#5107) Signed-off-by: Mamoru Sobue --- .../src/debug.cpp | 46 ++- .../src/scene_intersection.cpp | 373 ++++++++---------- .../src/util_type.hpp | 24 +- 3 files changed, 202 insertions(+), 241 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index ae8eee39d6556..55987600a22dc 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -104,7 +104,33 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( return msg; } -visualization_msgs::msg::Marker createPointMarkerArray( +visualization_msgs::msg::MarkerArray createLineMarkerArray( + const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end, + const std::string & ns, const int64_t id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = ns + "_line"; + marker.id = id; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + geometry_msgs::msg::Vector3 arrow; + arrow.x = 1.0; + arrow.y = 1.0; + arrow.z = 1.0; + marker.scale = arrow; + marker.color = createMarkerColor(r, g, b, 0.999); + marker.points.push_back(point_start); + marker.points.push_back(point_end); + + msg.markers.push_back(marker); + return msg; +} + +[[maybe_unused]] visualization_msgs::msg::Marker createPointMarkerArray( const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r, const double g, const double b) { @@ -176,17 +202,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } - if (debug_data_.nearest_occlusion_point) { - debug_marker_array.markers.push_back(createPointMarkerArray( - debug_data_.nearest_occlusion_point.value(), "nearest_occlusion", module_id_, 0.5, 0.5, 0.0)); - } - - if (debug_data_.nearest_occlusion_projection_point) { - debug_marker_array.markers.push_back(createPointMarkerArray( - debug_data_.nearest_occlusion_projection_point.value(), "nearest_occlusion_projection", - module_id_, 0.5, 0.5, 0.0)); - } - size_t i{0}; for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( @@ -229,6 +244,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.nearest_occlusion_projection) { + const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); + appendMarkerArray( + createLineMarkerArray( + point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), + &debug_marker_array, now); + } return debug_marker_array; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index f34c828db0f13..2ae710f9be5ba 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -19,15 +19,19 @@ #include #include #include -#include -#include -#include #include -#include +#include #include +#include +#include +#include +#include +#include + +#include #include -#include +#include #include #include @@ -1172,7 +1176,8 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const std::vector & parked_attention_objects, + [[maybe_unused]] const std::vector & + blocking_attention_objects, const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; @@ -1190,19 +1195,13 @@ bool IntersectionModule::isOcclusionCleared( first_inside_attention_idx_ip_opt ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) : lane_interval_ip; + const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; const int width = occ_grid.info.width; const int height = occ_grid.info.height; const double resolution = occ_grid.info.resolution; const auto & origin = occ_grid.info.origin.position; - // NOTE: interesting area is set to 0 for later masking - cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); - cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); - - // (1) prepare detection area mask - // attention: 255 - // non-attention: 0 Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); @@ -1212,10 +1211,9 @@ bool IntersectionModule::isOcclusionCleared( grid_poly.outer().emplace_back(origin.x, origin.y); bg::correct(grid_poly); - std::vector> attention_area_cv_polygons; - for (const auto & attention_area : attention_areas) { - const auto area2d = lanelet::utils::to2D(attention_area); - Polygon2d area2d_poly; + auto findCommonCvPolygons = + [&](const auto & area2d, std::vector> & cv_polygons) -> void { + tier4_autoware_utils::Polygon2d area2d_poly; for (const auto & p : area2d) { area2d_poly.outer().emplace_back(p.x(), p.y()); } @@ -1224,21 +1222,32 @@ bool IntersectionModule::isOcclusionCleared( std::vector common_areas; bg::intersection(area2d_poly, grid_poly, common_areas); if (common_areas.empty()) { - continue; + return; } for (size_t i = 0; i < common_areas.size(); ++i) { common_areas[i].outer().push_back(common_areas[i].outer().front()); bg::correct(common_areas[i]); } for (const auto & common_area : common_areas) { - std::vector attention_area_cv_polygon; + std::vector cv_polygon; for (const auto & p : common_area.outer()) { const int idx_x = static_cast((p.x() - origin.x) / resolution); const int idx_y = static_cast((p.y() - origin.y) / resolution); - attention_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + cv_polygon.emplace_back(idx_x, height - 1 - idx_y); } - attention_area_cv_polygons.push_back(attention_area_cv_polygon); + cv_polygons.push_back(cv_polygon); } + }; + + // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 + // NOTE: interesting area is set to 255 for later masking + cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); + std::vector> attention_area_cv_polygons; + for (const auto & attention_area : attention_areas) { + const auto area2d = lanelet::utils::to2D(attention_area); + findCommonCvPolygons(area2d, attention_area_cv_polygons); } for (const auto & poly : attention_area_cv_polygons) { cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_4); @@ -1248,30 +1257,7 @@ bool IntersectionModule::isOcclusionCleared( std::vector> adjacent_lane_cv_polygons; for (const auto & adjacent_lanelet : adjacent_lanelets) { const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); - Polygon2d area2d_poly; - for (const auto & p : area2d) { - area2d_poly.outer().emplace_back(p.x(), p.y()); - } - area2d_poly.outer().push_back(area2d_poly.outer().front()); - bg::correct(area2d_poly); - std::vector common_areas; - bg::intersection(area2d_poly, grid_poly, common_areas); - if (common_areas.empty()) { - continue; - } - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - for (const auto & common_area : common_areas) { - std::vector adjacent_lane_cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = static_cast((p.x() - origin.x) / resolution); - const int idx_y = static_cast((p.y() - origin.y) / resolution); - adjacent_lane_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - adjacent_lane_cv_polygons.push_back(adjacent_lane_cv_polygon); - } + findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); } for (const auto & poly : adjacent_lane_cv_polygons) { cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); @@ -1279,6 +1265,10 @@ bool IntersectionModule::isOcclusionCleared( // (2) prepare unknown mask // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] + // unknown: 255 + // not-unknown: 0 + cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); + cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); for (int x = 0; x < width; x++) { for (int y = 0; y < height; y++) { const int idx = y * width + x; @@ -1286,159 +1276,21 @@ bool IntersectionModule::isOcclusionCleared( if ( planner_param_.occlusion.free_space_max <= intensity && intensity < planner_param_.occlusion.occupied_min) { - unknown_mask.at(height - 1 - y, x) = 255; + unknown_mask_raw.at(height - 1 - y, x) = 255; } } } - - // (3) occlusion mask - cv::Mat occlusion_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); - cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask_raw); - // (3.1) apply morphologyEx - cv::Mat occlusion_mask; - const int morph_size = std::ceil(planner_param_.occlusion.denoise_kernel / resolution); + // (2.1) apply morphologyEx + const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); cv::morphologyEx( - occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN, + unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); - // (4) create distance grid - // value: 0 - 254: signed distance representing [distance_min, distance_max] - // 255: undefined value - const double distance_max = std::hypot(width * resolution / 2, height * resolution / 2); - const double distance_min = -distance_max; - const int undef_pixel = 255; - const int max_cost_pixel = 254; - auto dist2pixel = [=](const double dist) { - return std::min( - max_cost_pixel, - static_cast((dist - distance_min) / (distance_max - distance_min) * max_cost_pixel)); - }; - auto pixel2dist = [=](const int pixel) { - return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min; - }; - const int zero_dist_pixel = dist2pixel(0.0); - const int parked_vehicle_pixel = zero_dist_pixel - 1; // magic - - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - - cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel)); - cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1)); - - // (4.1) fill zero_dist_pixel on the path - const auto [lane_start, lane_end] = lane_attention_interval_ip; - for (int i = static_cast(lane_end); i >= static_cast(lane_start); i--) { - const auto & path_pos = path_ip.points.at(i).point.pose.position; - const int idx_x = (path_pos.x - origin.x) / resolution; - const int idx_y = (path_pos.y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) continue; - if (idx_y < 0 || idx_y >= height) continue; - distance_grid.at(height - 1 - idx_y, idx_x) = zero_dist_pixel; - projection_ind_grid.at(height - 1 - idx_y, idx_x) = i; - } - - // (4.2) fill parked_vehicle_pixel to parked_vehicles (both positive and negative) - for (const auto & parked_attention_object : parked_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(parked_attention_object); - std::vector common_areas; - bg::intersection(obj_poly, grid_poly, common_areas); - if (common_areas.empty()) continue; - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - std::vector> parked_attention_object_area_cv_polygons; - for (const auto & common_area : common_areas) { - std::vector parked_attention_object_area_cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = static_cast((p.x() - origin.x) / resolution); - const int idx_y = static_cast((p.y() - origin.y) / resolution); - parked_attention_object_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - parked_attention_object_area_cv_polygons.push_back(parked_attention_object_area_cv_polygon); - } - for (const auto & poly : parked_attention_object_area_cv_polygons) { - cv::fillPoly(distance_grid, poly, cv::Scalar(parked_vehicle_pixel), cv::LINE_AA); - } - } - - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - for (const auto & division : divisions) { - bool is_in_grid = false; - bool zero_dist_cell_found = false; - int projection_ind = -1; - std::optional> cost_prev_checkpoint = - std::nullopt; // cost, x, y, projection_ind - for (auto point = division.begin(); point != division.end(); point++) { - const double x = point->x(), y = point->y(); - const auto [valid, idx_x, idx_y] = coord2index(x, y); - // exited grid just now - if (is_in_grid && !valid) break; - - // still not entering grid - if (!is_in_grid && !valid) continue; - - // From here, "valid" - const int pixel = distance_grid.at(height - 1 - idx_y, idx_x); - - // entered grid for 1st time - if (!is_in_grid) { - assert(pixel == undef_pixel || pixel == zero_dist_pixel); - is_in_grid = true; - if (pixel == undef_pixel) { - continue; - } - } - - if (pixel == zero_dist_pixel) { - zero_dist_cell_found = true; - projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); - assert(projection_ind >= 0); - cost_prev_checkpoint = - std::make_optional>(0.0, x, y, projection_ind); - continue; - } - - // hit positive parked vehicle - if (zero_dist_cell_found && pixel == parked_vehicle_pixel) { - while (point != division.end()) { - const double x = point->x(), y = point->y(); - const auto [valid, idx_x, idx_y] = coord2index(x, y); - if (valid) occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - point++; - } - break; - } - - if (zero_dist_cell_found) { - // finally traversed to defined cell (first half) - const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] = - cost_prev_checkpoint.value(); - const double dy = y - prev_checkpoint_y, dx = x - prev_checkpoint_x; - double new_dist = prev_cost + std::hypot(dy, dx); - const int new_projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); - const double cur_dist = pixel2dist(pixel); - if (planner_param_.occlusion.do_dp && cur_dist < new_dist) { - new_dist = cur_dist; - if (new_projection_ind > 0) { - projection_ind = std::min(prev_projection_ind, new_projection_ind); - } - } - projection_ind_grid.at(height - 1 - idx_y, idx_x) = projection_ind; - distance_grid.at(height - 1 - idx_y, idx_x) = dist2pixel(new_dist); - cost_prev_checkpoint = std::make_optional>( - new_dist, x, y, projection_ind); - } - } - } - } + // (3) occlusion mask + cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // (4) extract occlusion polygons const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; @@ -1447,19 +1299,12 @@ bool IntersectionModule::isOcclusionCleared( cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector> valid_contours; for (const auto & contour : contours) { - std::vector valid_contour; - for (const auto & p : contour) { - if (distance_grid.at(p.y, p.x) == undef_pixel) { - continue; - } - valid_contour.push_back(p); - } - if (valid_contour.size() <= 2) { + if (contour.size() <= 2) { continue; } std::vector approx_contour; cv::approxPolyDP( - valid_contour, approx_contour, + contour, approx_contour, std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); if (approx_contour.size() <= 2) continue; // check area @@ -1486,32 +1331,126 @@ bool IntersectionModule::isOcclusionCleared( } debug_data_.occlusion_polygons.push_back(polygon_msg); } + // (4.1) re-draw occluded cells using valid_contours + occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + for (unsigned i = 0; i < valid_contours.size(); ++i) { + // NOTE: drawContour does not work well + cv::fillPoly(occlusion_mask, valid_contours[i], cv::Scalar(255), cv::LINE_AA); + } + + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; + + // (5) find distance + // (5.1) discretize path_ip with resolution for computational cost + LineString2d path_linestring; + path_linestring.emplace_back( + path_ip.points.at(lane_start_idx).point.pose.position.x, + path_ip.points.at(lane_start_idx).point.pose.position.y); + { + auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; + for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { + const auto path_linestring_point = path_ip.points.at(i).point.pose.position; + if ( + tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + 1.0 /* rough tick for computational cost */) { + continue; + } + path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); + prev_path_linestring_point = path_linestring_point; + } + } - const int min_cost_thr = dist2pixel(occlusion_dist_thr); - int min_cost = undef_pixel - 1; - geometry_msgs::msg::Point nearest_occlusion_point; - for (const auto & occlusion_contour : valid_contours) { - for (const auto & p : occlusion_contour) { - const int pixel = static_cast(distance_grid.at(p.y, p.x)); - const bool occluded = (occlusion_mask.at(p.y, p.x) == 255); - if (pixel == undef_pixel || !occluded) { + 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; + } + } + 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()) { continue; } - if (pixel < min_cost) { - min_cost = pixel; - nearest_occlusion_point.x = origin.x + p.x * resolution; - nearest_occlusion_point.y = origin.y + (height - 1 - p.y) * resolution; - nearest_occlusion_point.z = origin.z; + 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; + if (occlusion_mask.at(height - 1 - idx_y, idx_x) == 255) { + 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 (min_cost > min_cost_thr) { + if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { return true; - } else { - debug_data_.nearest_occlusion_point = nearest_occlusion_point; - return false; } + debug_data_.nearest_occlusion_projection = + std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); + return false; } /* diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index a4a0ed88217f7..6ee372eda063c 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -34,22 +34,22 @@ namespace behavior_velocity_planner::util struct DebugData { - std::optional collision_stop_wall_pose = std::nullopt; - std::optional occlusion_stop_wall_pose = std::nullopt; - std::optional occlusion_first_stop_wall_pose = std::nullopt; - std::optional pass_judge_wall_pose = std::nullopt; - std::optional> attention_area = std::nullopt; - std::optional intersection_area = std::nullopt; - std::optional ego_lane = std::nullopt; - std::optional> adjacent_area = std::nullopt; - std::optional stuck_vehicle_detect_area = std::nullopt; - std::optional candidate_collision_ego_lane_polygon = std::nullopt; + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + std::optional occlusion_first_stop_wall_pose{std::nullopt}; + std::optional pass_judge_wall_pose{std::nullopt}; + std::optional> attention_area{std::nullopt}; + std::optional intersection_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional stuck_vehicle_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; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - std::optional nearest_occlusion_point = std::nullopt; - std::optional nearest_occlusion_projection_point = std::nullopt; std::vector occlusion_polygons; + std::optional> + nearest_occlusion_projection{std::nullopt}; }; struct InterpolatedPathInfo