Skip to content

Commit

Permalink
fix(intersection_occlusion): estimate dynamic occlusion source with t…
Browse files Browse the repository at this point in the history
…riangle (autowarefoundation#6585)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Signed-off-by: kaigohirao <kaigo.hirao@proxima-ai-tech.com>
  • Loading branch information
soblin authored and kaigohirao committed Mar 22, 2024
1 parent 0c52f1e commit eb5f952
Show file tree
Hide file tree
Showing 3 changed files with 51 additions and 7 deletions.
16 changes: 16 additions & 0 deletions planning/behavior_velocity_intersection_module/src/debug.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -389,6 +389,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
&debug_marker_array, now);
}

if (debug_data_.nearest_occlusion_triangle) {
const auto [p1, p2, p3] = debug_data_.nearest_occlusion_triangle.value();
const auto color = debug_data_.static_occlusion ? green : red;
geometry_msgs::msg::Polygon poly;
poly.points.push_back(
geometry_msgs::build<geometry_msgs::msg::Point32>().x(p1.x).y(p1.y).z(p1.z));
poly.points.push_back(
geometry_msgs::build<geometry_msgs::msg::Point32>().x(p2.x).y(p2.y).z(p2.z));
poly.points.push_back(
geometry_msgs::build<geometry_msgs::msg::Point32>().x(p3.x).y(p3.y).z(p3.z));
appendMarkerArray(
debug::createPolygonMarkerArray(
poly, "nearest_occlusion_triangle", lane_id_, now, 0.3, 0.0, 0.0, std::get<0>(color),
std::get<1>(color), std::get<2>(color)),
&debug_marker_array, now);
}
if (debug_data_.traffic_light_observation) {
const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN;
const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -235,6 +235,10 @@ class IntersectionModule : public SceneModuleInterface
std::vector<geometry_msgs::msg::Polygon> occlusion_polygons;
std::optional<std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
nearest_occlusion_projection{std::nullopt};
std::optional<
std::tuple<geometry_msgs::msg::Point, geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
nearest_occlusion_triangle{std::nullopt};
bool static_occlusion{false};
std::optional<double> static_occlusion_with_traffic_light_timeout{std::nullopt};

std::optional<std::tuple<geometry_msgs::msg::Pose, lanelet::ConstPoint3d, lanelet::Id, uint8_t>>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -334,13 +334,14 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
}
return nearest;
};
struct NearestOcclusionPoint
struct NearestOcclusionInterval
{
int64 division_index{0};
int64 point_index{0};
double dist{0.0};
geometry_msgs::msg::Point point;
geometry_msgs::msg::Point projection;
geometry_msgs::msg::Point visible_end;
} nearest_occlusion_point;
double min_dist = std::numeric_limits<double>::infinity();
for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) {
Expand Down Expand Up @@ -370,6 +371,8 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
continue;
}
double acc_dist = 0.0;
bool found_min_dist_for_this_division = false;
bool is_prev_occluded = false;
auto acc_dist_it = projection_it;
for (auto point_it = projection_it; point_it != division.end(); point_it++) {
const double dist =
Expand All @@ -386,11 +389,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
if (acc_dist < min_dist) {
min_dist = acc_dist;
nearest_occlusion_point = {
division_index, std::distance(division.begin(), point_it), acc_dist,
division_index,
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)};
tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z),
tier4_autoware_utils::createPoint(
projection_it->x(), projection_it->y(),
origin.z) /* initialize with projection point at first*/};
found_min_dist_for_this_division = true;
} else if (found_min_dist_for_this_division && is_prev_occluded) {
// although this cell is not "nearest" cell, we have found the "nearest" cell on this
// division previously in this iteration, and the iterated cells are still OCCLUDED since
// then
nearest_occlusion_point.visible_end =
tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z);
}
}
is_prev_occluded = (pixel == OCCLUDED);
}
}

Expand All @@ -400,16 +416,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(

debug_data_.nearest_occlusion_projection =
std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection);
LineString2d ego_occlusion_line;
ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y);
ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y);
debug_data_.nearest_occlusion_triangle = std::make_tuple(
current_pose.position, nearest_occlusion_point.point, nearest_occlusion_point.visible_end);
Polygon2d ego_occlusion_triangle;
ego_occlusion_triangle.outer().emplace_back(current_pose.position.x, current_pose.position.y);
ego_occlusion_triangle.outer().emplace_back(
nearest_occlusion_point.point.x, nearest_occlusion_point.point.y);
ego_occlusion_triangle.outer().emplace_back(
nearest_occlusion_point.visible_end.x, nearest_occlusion_point.visible_end.y);
bg::correct(ego_occlusion_triangle);
for (const auto & attention_object_info : object_info_manager_.allObjects()) {
const auto obj_poly =
tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object());
if (bg::intersects(obj_poly, ego_occlusion_line)) {
if (bg::intersects(obj_poly, ego_occlusion_triangle)) {
debug_data_.static_occlusion = false;
return DynamicallyOccluded{min_dist};
}
}
debug_data_.static_occlusion = true;
return StaticallyOccluded{min_dist};
}
} // namespace behavior_velocity_planner

0 comments on commit eb5f952

Please sign in to comment.