Skip to content

Commit

Permalink
feat(map_based_prediction): cope with consecutive crosswalks (autowar…
Browse files Browse the repository at this point in the history 
…efoundation#6009)

* ready for consecutive crosswalks

Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
@yuki-takagi-66 @karishma1911
yuki-takagi-66 authored and karishma1911 committed May 28, 2024
1 parent cf560cc commit 5de9aab
Showing 1 changed file with 86 additions and 81 deletions.
167 changes: 86 additions & 81 deletions perception/map_based_prediction/src/map_based_prediction_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -415,17 +415,21 @@ bool withinRoadLanelet(
}

boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
const TrackedObject & object, const CrosswalkEdgePoints & edge_points,
const lanelet::LaneletMapPtr & lanelet_map_ptr, const double time_horizon,
const double min_object_vel)
const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk,
const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr,
const double time_horizon, const double min_object_vel)
{
using Point = boost::geometry::model::d2::point_xy<double>;
using Line = boost::geometry::model::linestring<Point>;

const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear;
const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z;

lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y);
if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) {
return {};
}

const auto & p1 = edge_points.front_center_point;
const auto & p2 = edge_points.back_center_point;

Expand All @@ -442,62 +446,66 @@ boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
const auto is_stop_object = estimated_velocity < stop_velocity_th;
const auto velocity = std::max(min_object_vel, estimated_velocity);

lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
// nearest lanelet
const auto surrounding_lanelets = lanelet::geometry::findNearest(
lanelet_map_ptr->laneletLayer, search_point, time_horizon * velocity);
lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity);

bool first_intersect_load = false;
bool second_intersect_load = false;
std::vector<Point> intersects_first;
std::vector<Point> intersects_second;
for (const auto & lanelet : surrounding_lanelets) {
if (withinLanelet(object, lanelet.second)) {
return {};
}
const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) {
const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) {
for (const auto & lanelet : surrounding_lanelets) {
const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
if (
(attr.value() == lanelet::AttributeValueString::Crosswalk ||
attr.value() == lanelet::AttributeValueString::Walkway) &&
boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) {
return true;
}
}
return false;
};

lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
if (attr.value() != "road") {
continue;
}
const auto isExist = [](const Point & p, const std::vector<Point> & points) {
for (const auto & existingPoint : points) {
if (boost::geometry::distance(p, existingPoint) < 1e-1) {
return true;
}
}
return false;
};

{
const Line object_to_entry_point{
{obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}};
std::vector<Point> tmp_intersects;
boost::geometry::intersection(
object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects);
for (const auto & p : tmp_intersects) {
intersects_first.push_back(p);
std::vector<Point> points_of_intersect;
const boost::geometry::model::linestring<Point> line{p_src, p_dst};
for (const auto & lanelet : surrounding_lanelets) {
const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
if (attr.value() != lanelet::AttributeValueString::Road) {
continue;
}
}

{
const Line object_to_entry_point{
{obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}};
std::vector<Point> tmp_intersects;
boost::geometry::intersection(
object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects);
line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects);
for (const auto & p : tmp_intersects) {
intersects_second.push_back(p);
if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) {
continue;
}
points_of_intersect.push_back(p);
if (points_of_intersect.size() >= 2) {
return true;
}
}
}
}

if (1 < intersects_first.size()) {
first_intersect_load = true;
}
return false;
};

if (1 < intersects_second.size()) {
second_intersect_load = true;
}
const bool first_intersects_road = isAcrossAnyRoad(
{obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()});
const bool second_intersects_road =
isAcrossAnyRoad({obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()});

if (first_intersect_load && second_intersect_load) {
if (first_intersects_road && second_intersects_road) {
return {};
}

if (first_intersect_load && !second_intersect_load) {
if (first_intersects_road && !second_intersects_road) {
ret.swap();
}

Expand Down Expand Up @@ -1197,48 +1205,45 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}
}
}
// try to find the edge points for all crosswalks and generate path to the crosswalk edge
for (const auto & crosswalk : crosswalks_) {
const auto edge_points = getCrosswalkEdgePoints(crosswalk);

const auto reachable_first = hasPotentialToReach(
object, edge_points.front_center_point, edge_points.front_right_point,
edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_,
max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
const auto reachable_second = hasPotentialToReach(
object, edge_points.back_center_point, edge_points.back_right_point,
edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_,
max_crosswalk_user_delta_yaw_threshold_for_lanelet_);

if (!reachable_first && !reachable_second) {
continue;
}

// If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge
// points for all crosswalks and generate path to the crosswalk edge
} else {
for (const auto & crosswalk : crosswalks_) {
const auto edge_points = getCrosswalkEdgePoints(crosswalk);

const auto reachable_first = hasPotentialToReach(
object, edge_points.front_center_point, edge_points.front_right_point,
edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_,
max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
const auto reachable_second = hasPotentialToReach(
object, edge_points.back_center_point, edge_points.back_right_point,
edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_,
max_crosswalk_user_delta_yaw_threshold_for_lanelet_);

if (!reachable_first && !reachable_second) {
continue;
}

const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
object, edge_points, lanelet_map_ptr_, prediction_time_horizon_,
min_crosswalk_user_velocity_);

if (!reachable_crosswalk) {
continue;
}
const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_,
min_crosswalk_user_velocity_);

PredictedPath predicted_path =
path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get());
predicted_path.confidence = 1.0;
if (!reachable_crosswalk) {
continue;
}

if (predicted_path.path.empty()) {
continue;
}
// If the predicted path to the crosswalk is crossing the fence, don't use it
if (doesPathCrossAnyFence(predicted_path)) {
continue;
}
PredictedPath predicted_path =
path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get());
predicted_path.confidence = 1.0;

predicted_object.kinematics.predicted_paths.push_back(predicted_path);
if (predicted_path.path.empty()) {
continue;
}
// If the predicted path to the crosswalk is crossing the fence, don't use it
if (doesPathCrossAnyFence(predicted_path)) {
continue;
}

predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}

const auto n_path = predicted_object.kinematics.predicted_paths.size();
Expand Down

0 comments on commit 5de9aab

Please sign in to comment.