feat(intersection): cherry-pick latest changes

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -13,6 +13,10 @@
         path_interpolation_ds: 0.1 # [m]
         consider_wrong_direction_vehicle: false
       stuck_vehicle:
+        turn_direction:
+          left: true
+          right: true
+          straight: true
         use_stuck_stopline: true # stopline generated before the first conflicting area
         stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
         stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -63,6 +63,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.common.consider_wrong_direction_vehicle =
     getOrDeclareParameter<bool>(node, ns + ".common.consider_wrong_direction_vehicle");
 
+  ip.stuck_vehicle.turn_direction.left =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.turn_direction.left");
+  ip.stuck_vehicle.turn_direction.right =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.turn_direction.right");
+  ip.stuck_vehicle.turn_direction.straight =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.turn_direction.straight");
   ip.stuck_vehicle.use_stuck_stopline =
     getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.use_stuck_stopline");
   ip.stuck_vehicle.stuck_vehicle_detect_dist =

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -25,6 +25,7 @@
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
+#include <boost/geometry/algorithms/convex_hull.hpp>
 #include <boost/geometry/algorithms/correct.hpp>
 #include <boost/geometry/algorithms/intersection.hpp>
 
@@ -43,6 +44,32 @@ namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
 
+namespace
+{
+Polygon2d createOneStepPolygon(
+  const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose,
+  const autoware_auto_perception_msgs::msg::Shape & shape)
+{
+  const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape);
+  const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape);
+
+  Polygon2d one_step_poly;
+  for (const auto & point : prev_poly.outer()) {
+    one_step_poly.outer().push_back(point);
+  }
+  for (const auto & point : next_poly.outer()) {
+    one_step_poly.outer().push_back(point);
+  }
+
+  bg::correct(one_step_poly);
+
+  Polygon2d convex_one_step_poly;
+  bg::convex_hull(one_step_poly, convex_one_step_poly);
+
+  return convex_one_step_poly;
+}
+}  // namespace
+
 static bool isTargetCollisionVehicleType(
   const autoware_auto_perception_msgs::msg::PredictedObject & object)
 {
@@ -877,7 +904,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map);
   const bool is_prioritized =
     traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED;
-  intersection_lanelets.update(is_prioritized, interpolated_path_info);
+  const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
+  intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint);
 
   // this is abnormal
   const auto & conflicting_lanelets = intersection_lanelets.conflicting();
@@ -1185,6 +1213,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 bool IntersectionModule::checkStuckVehicle(
   const std::shared_ptr<const PlannerData> & planner_data, const util::PathLanelets & path_lanelets)
 {
+  const bool stuck_detection_direction = [&]() {
+    return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) ||
+           (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) ||
+           (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight);
+  }();
+  if (!stuck_detection_direction) {
+    return false;
+  }
+
   const auto & objects_ptr = planner_data->predicted_objects;
 
   // considering lane change in the intersection, these lanelets are generated from the path
@@ -1312,14 +1349,14 @@ bool IntersectionModule::checkCollision(
       // collision point
       const auto first_itr = std::adjacent_find(
         predicted_path.path.cbegin(), predicted_path.path.cend(),
-        [&ego_poly](const auto & a, const auto & b) {
-          return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)});
+        [&ego_poly, &object](const auto & a, const auto & b) {
+          return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape));
         });
       if (first_itr == predicted_path.path.cend()) continue;
       const auto last_itr = std::adjacent_find(
         predicted_path.path.crbegin(), predicted_path.path.crend(),
-        [&ego_poly](const auto & a, const auto & b) {
-          return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)});
+        [&ego_poly, &object](const auto & a, const auto & b) {
+          return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape));
         });
       if (last_itr == predicted_path.path.crend()) continue;
 

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -62,6 +62,12 @@ class IntersectionModule : public SceneModuleInterface
     } common;
     struct StuckVehicle
     {
+      struct TurnDirection
+      {
+        bool left;
+        bool right;
+        bool straight;
+      } turn_direction;
       bool use_stuck_stopline;  //! stopline generate before the intersection lanelet when is_stuck.
       double stuck_vehicle_detect_dist;  //! distance from end point to finish stuck vehicle check
       double stuck_vehicle_vel_thr;      //! Threshold of the speed to be recognized as stopped

planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp

@@ -93,8 +93,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
       planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length,
       planner_param_.consider_wrong_direction_vehicle);
   }
-  intersection_lanelets_.value().update(false, interpolated_path_info);
-  const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area();
+  auto & intersection_lanelets = intersection_lanelets_.value();
+  const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
+  intersection_lanelets.update(false, interpolated_path_info, local_footprint);
+  const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area();
   if (!first_conflicting_area) {
     return false;
   }

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -217,6 +217,50 @@ static std::optional<size_t> getStopLineIndexFromMap(
   return stop_idx_ip_opt.get();
 }
 
+static std::optional<size_t> getFirstPointInsidePolygonByFootprint(
+  const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info,
+  const tier4_autoware_utils::LinearRing2d & footprint)
+{
+  const auto & path_ip = interpolated_path_info.path;
+  const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value();
+
+  const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon();
+  for (auto i = lane_start; i <= lane_end; ++i) {
+    const auto & base_pose = path_ip.points.at(i).point.pose;
+    const auto path_footprint = tier4_autoware_utils::transformVector(
+      footprint, tier4_autoware_utils::pose2transform(base_pose));
+    if (bg::intersects(path_footprint, area_2d)) {
+      return std::make_optional<size_t>(i);
+    }
+  }
+  return std::nullopt;
+}
+
+static std::optional<std::pair<size_t, const lanelet::CompoundPolygon3d &>>
+getFirstPointInsidePolygonsByFootprint(
+  const std::vector<lanelet::CompoundPolygon3d> & polygons,
+  const InterpolatedPathInfo & interpolated_path_info,
+  const tier4_autoware_utils::LinearRing2d & footprint)
+{
+  const auto & path_ip = interpolated_path_info.path;
+  const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value();
+
+  for (size_t i = lane_start; i <= lane_end; ++i) {
+    const auto & pose = path_ip.points.at(i).point.pose;
+    const auto path_footprint =
+      tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose));
+    for (const auto & polygon : polygons) {
+      const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon();
+      const bool is_in_polygon = bg::intersects(area_2d, path_footprint);
+      if (is_in_polygon) {
+        return std::make_optional<std::pair<size_t, const lanelet::CompoundPolygon3d &>>(
+          i, polygon);
+      }
+    }
+  }
+  return std::nullopt;
+}
+
 std::optional<IntersectionStopLines> generateIntersectionStopLines(
   const lanelet::CompoundPolygon3d & first_conflicting_area,
   const lanelet::CompoundPolygon3d & first_detection_area,
@@ -233,6 +277,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   const int base2front_idx_dist =
     std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds);
 
+  /*
   // find the index of the first point that intersects with detection_areas
   const auto first_inside_detection_idx_ip_opt =
     getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area);
@@ -241,6 +286,16 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
     return std::nullopt;
   }
   const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value();
+  */
+  // find the index of the first point whose vehicle footprint on it intersects with detection_area
+  const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0);
+  std::optional<size_t> first_footprint_inside_detection_ip_opt =
+    getFirstPointInsidePolygonByFootprint(
+      first_detection_area, interpolated_path_info, local_footprint);
+  if (!first_footprint_inside_detection_ip_opt) {
+    return std::nullopt;
+  }
+  const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value();
 
   // (1) default stop line position on interpolated path
   bool default_stop_line_valid = true;
@@ -251,8 +306,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
     stop_idx_ip_int = static_cast<int>(map_stop_idx_ip.value()) - base2front_idx_dist;
   }
   if (stop_idx_ip_int < 0) {
-    stop_idx_ip_int = static_cast<size_t>(first_inside_detection_ip) - stop_line_margin_idx_dist -
-                      base2front_idx_dist;
+    stop_idx_ip_int = first_footprint_inside_detection_ip - stop_line_margin_idx_dist;
   }
   if (stop_idx_ip_int < 0) {
     default_stop_line_valid = false;
@@ -269,29 +323,20 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   const auto closest_idx_ip = closest_idx_ip_opt.value();
 
   // (3) occlusion peeking stop line position on interpolated path
-  const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0);
-  const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon();
   int occlusion_peeking_line_ip_int = static_cast<int>(default_stop_line_ip);
   bool occlusion_peeking_line_valid = true;
   {
     // NOTE: if footprints[0] is already inside the detection area, invalid
     const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose;
     const auto path_footprint0 = tier4_autoware_utils::transformVector(
       local_footprint, tier4_autoware_utils::pose2transform(base_pose0));
-    if (bg::intersects(path_footprint0, area_2d)) {
+    if (bg::intersects(
+          path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) {
       occlusion_peeking_line_valid = false;
     }
   }
   if (occlusion_peeking_line_valid) {
-    for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) {
-      const auto & base_pose = path_ip.points.at(i).point.pose;
-      const auto path_footprint = tier4_autoware_utils::transformVector(
-        local_footprint, tier4_autoware_utils::pose2transform(base_pose));
-      if (bg::intersects(path_footprint, area_2d)) {
-        occlusion_peeking_line_ip_int = i;
-        break;
-      }
-    }
+    occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip;
   }
   const auto first_attention_stop_line_ip = static_cast<size_t>(occlusion_peeking_line_ip_int);
   const bool first_attention_stop_line_valid = occlusion_peeking_line_valid;
@@ -307,8 +352,8 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   const double delay_response_time = planner_data->delay_response_time;
   const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit(
     velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time);
-  int pass_judge_ip_int = static_cast<int>(first_inside_detection_ip) - base2front_idx_dist -
-                          std::ceil(braking_dist / ds);
+  int pass_judge_ip_int =
+    static_cast<int>(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds);
   const auto pass_judge_line_ip = static_cast<size_t>(
     std::clamp<int>(pass_judge_ip_int, 0, static_cast<int>(path_ip.points.size()) - 1));
 
@@ -318,18 +363,18 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   if (use_stuck_stopline) {
     // NOTE: when ego vehicle is approaching detection area and already passed
     // first_conflicting_area, this could be null.
-    const auto stuck_stop_line_idx_ip_opt =
-      getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area);
+    const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint(
+      first_conflicting_area, interpolated_path_info, local_footprint);
     if (!stuck_stop_line_idx_ip_opt) {
       stuck_stop_line_valid = false;
       stuck_stop_line_ip_int = 0;
     } else {
-      stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value();
+      stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist;
     }
   } else {
-    stuck_stop_line_ip_int = std::get<0>(lane_interval_ip);
+    stuck_stop_line_ip_int =
+      std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist);
   }
-  stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist);
   if (stuck_stop_line_ip_int < 0) {
     stuck_stop_line_valid = false;
   }
@@ -427,6 +472,9 @@ std::optional<size_t> getFirstPointInsidePolygon(
       if (is_in_lanelet) {
         return std::make_optional<size_t>(i);
       }
+      if (i == 0) {
+        break;
+      }
     }
   }
   return std::nullopt;
@@ -469,6 +517,9 @@ getFirstPointInsidePolygons(
       if (is_in_lanelet) {
         break;
       }
+      if (i == 0) {
+        break;
+      }
     }
   }
   return std::nullopt;
@@ -1240,21 +1291,22 @@ double calcDistanceUntilIntersectionLanelet(
 }
 
 void IntersectionLanelets::update(
-  const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info)
+  const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info,
+  const tier4_autoware_utils::LinearRing2d & footprint)
 {
   is_prioritized_ = is_prioritized;
   // find the first conflicting/detection area polygon intersecting the path
-  const auto & path = interpolated_path_info.path;
-  const auto & lane_interval = interpolated_path_info.lane_id_interval.value();
-  {
-    auto first = getFirstPointInsidePolygons(path, lane_interval, conflicting_area_);
-    if (first && !first_conflicting_area_) {
+  if (!first_conflicting_area_) {
+    auto first =
+      getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint);
+    if (first) {
       first_conflicting_area_ = first.value().second;
     }
   }
-  {
-    auto first = getFirstPointInsidePolygons(path, lane_interval, attention_area_);
-    if (first && !first_attention_area_) {
+  if (!first_attention_area_) {
+    auto first =
+      getFirstPointInsidePolygonsByFootprint(attention_area_, interpolated_path_info, footprint);
+    if (first) {
       first_attention_area_ = first.value().second;
     }
   }

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -15,6 +15,8 @@
 #ifndef UTIL_TYPE_HPP_
 #define UTIL_TYPE_HPP_
 
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -67,7 +69,9 @@ struct InterpolatedPathInfo
 struct IntersectionLanelets
 {
 public:
-  void update(const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info);
+  void update(
+    const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info,
+    const tier4_autoware_utils::LinearRing2d & footprint);
   const lanelet::ConstLanelets & attention() const
   {
     return is_prioritized_ ? attention_non_preceding_ : attention_;