feat(map_based_prediction): cope with consecutive crosswalks (autowarefoundation#6009)

* ready for consecutive crosswalks

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -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;
 
@@ -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();
   }
 
@@ -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();