Merge pull request #1028 from tier4/fix/intersection_processing_time

fix(intersection): reduce processing time and fix wrong resample process

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -1421,6 +1421,37 @@ static lanelet::ConstLanelets getPrevLanelets(
   return previous_lanelets;
 }
 
+// end inclusive
+lanelet::ConstLanelet generatePathLanelet(
+  const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width,
+  const double interval)
+{
+  lanelet::Points3d lefts;
+  lanelet::Points3d rights;
+  size_t prev_idx = start_idx;
+  for (size_t i = start_idx; i <= end_idx; ++i) {
+    const auto & p = path.points.at(i).point.pose;
+    const auto & p_prev = path.points.at(prev_idx).point.pose;
+    if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) {
+      continue;
+    }
+    prev_idx = i;
+    const double yaw = tf2::getYaw(p.orientation);
+    const double x = p.position.x;
+    const double y = p.position.y;
+    const double left_x = x + width / 2 * std::sin(yaw);
+    const double left_y = y - width / 2 * std::cos(yaw);
+    const double right_x = x - width / 2 * std::sin(yaw);
+    const double right_y = y + width / 2 * std::cos(yaw);
+    lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z);
+    rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z);
+  }
+  lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts);
+  lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights);
+
+  return lanelet::Lanelet(lanelet::InvalId, left, right);
+}
+
 std::optional<PathLanelets> generatePathLanelets(
   const lanelet::ConstLanelets & lanelets_on_path,
   const util::InterpolatedPathInfo & interpolated_path_info, const std::set<int> & associative_ids,
@@ -1430,6 +1461,8 @@ std::optional<PathLanelets> generatePathLanelets(
   const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx,
   const double width)
 {
+  static constexpr double path_lanelet_interval = 1.5;
+
   const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval;
   if (!assigned_lane_interval_opt) {
     return std::nullopt;
@@ -1446,13 +1479,13 @@ std::optional<PathLanelets> generatePathLanelets(
   const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval;
   if (closest_idx > assigned_lane_start) {
     path_lanelets.all.push_back(
-      planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width));
+      generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval));
   }
 
   // ego_or_entry2exit
   const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start);
   path_lanelets.ego_or_entry2exit =
-    planning_utils::generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width);
+    generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval);
   path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit);
 
   // next
@@ -1461,7 +1494,8 @@ std::optional<PathLanelets> generatePathLanelets(
     const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id});
     if (next_lane_interval_opt) {
       const auto [next_start, next_end] = next_lane_interval_opt.value();
-      path_lanelets.next = planning_utils::generatePathLanelet(path, next_start, next_end, width);
+      path_lanelets.next =
+        generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval);
       path_lanelets.all.push_back(path_lanelets.next.value());
     }
   }
@@ -1477,12 +1511,13 @@ std::optional<PathLanelets> generatePathLanelets(
   if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) {
     const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value();
     const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first;
-    lanelet::ConstLanelet conflicting_interval = planning_utils::generatePathLanelet(
-      path, first_inside_conflicting_idx, last_inside_conflicting_idx, width);
+    lanelet::ConstLanelet conflicting_interval = generatePathLanelet(
+      path, first_inside_conflicting_idx, last_inside_conflicting_idx, width,
+      path_lanelet_interval);
     path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval));
     if (last_inside_conflicting_idx < assigned_lane_end) {
-      lanelet::ConstLanelet remaining_interval = planning_utils::generatePathLanelet(
-        path, last_inside_conflicting_idx, assigned_lane_end, width);
+      lanelet::ConstLanelet remaining_interval = generatePathLanelet(
+        path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval);
       path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval));
     }
   }

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -154,6 +154,10 @@ double calcDistanceUntilIntersectionLanelet(
   const lanelet::ConstLanelet & assigned_lanelet,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx);
 
+lanelet::ConstLanelet generatePathLanelet(
+  const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width,
+  const double interval);
+
 std::optional<PathLanelets> generatePathLanelets(
   const lanelet::ConstLanelets & lanelets_on_path,
   const util::InterpolatedPathInfo & interpolated_path_info, const std::set<int> & associative_ids,

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp

@@ -185,8 +185,6 @@ geometry_msgs::msg::Pose getAheadPose(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
 Polygon2d generatePathPolygon(
   const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width);
-lanelet::ConstLanelet generatePathLanelet(
-  const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width);
 double calcJudgeLineDistWithAccLimit(
   const double velocity, const double max_stop_acceleration, const double delay_response_time);
 

planning/behavior_velocity_planner_common/src/utilization/util.cpp

@@ -287,32 +287,6 @@ Polygon2d generatePathPolygon(
   return ego_area;
 }
 
-lanelet::ConstLanelet generatePathLanelet(
-  const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width)
-{
-  lanelet::Points3d lefts;
-  for (size_t i = start_idx; i <= end_idx; ++i) {
-    const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation);
-    const double x = path.points.at(i).point.pose.position.x + width / 2 * std::sin(yaw);
-    const double y = path.points.at(i).point.pose.position.y - width / 2 * std::cos(yaw);
-    const lanelet::Point3d p(lanelet::InvalId, x, y, path.points.at(i).point.pose.position.z);
-    lefts.emplace_back(p);
-  }
-  lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts);
-
-  lanelet::Points3d rights;
-  for (size_t i = start_idx; i <= end_idx; ++i) {
-    const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation);
-    const double x = path.points.at(i).point.pose.position.x - width / 2 * std::sin(yaw);
-    const double y = path.points.at(i).point.pose.position.y + width / 2 * std::cos(yaw);
-    const lanelet::Point3d p(lanelet::InvalId, x, y, path.points.at(i).point.pose.position.z);
-    rights.emplace_back(p);
-  }
-  lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights);
-
-  return lanelet::Lanelet(lanelet::InvalId, left, right);
-}
-
 geometry_msgs::msg::Pose transformRelCoordinate2D(
   const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin)
 {