Merge pull request autowarefoundation#944 from tier4/feat/cherry-pick…

…-intersection-10-15

feat(intersection): cherry-pick latest changes

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -24,6 +24,12 @@
         # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
         timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area
         enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true.
+        yield_stuck:
+          turn_direction:
+            left: true
+            right: false
+            straight: false
+          distance_thr: 1.0 # [m]
 
       collision_detection:
         state_transit_margin_time: 1.0
@@ -44,7 +50,9 @@
         yield_on_green_traffic_light:
           distance_to_assigned_lanelet_start: 5.0
           duration: 2.0
-          range: 30.0 # [m]
+          object_dist_to_stopline: 10.0 # [m]
+        ignore_on_amber_traffic_light:
+          object_expected_deceleration: 2.0 # [m/ss]
 
       occlusion:
         enable: false

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -224,11 +224,21 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0),
     &debug_marker_array, now);
 
+  appendMarkerArray(
+    debug::createObjectsMarkerArray(
+      debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0),
+    &debug_marker_array, now);
+
   appendMarkerArray(
     debug::createObjectsMarkerArray(
       debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2),
     &debug_marker_array, now);
 
+  appendMarkerArray(
+    debug::createObjectsMarkerArray(
+      debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2),
+    &debug_marker_array, now);
+
   appendMarkerArray(
     debug::createObjectsMarkerArray(
       debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99,

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -85,6 +85,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.timeout_private_area");
   ip.stuck_vehicle.enable_private_area_stuck_disregard =
     getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard");
+  ip.stuck_vehicle.yield_stuck_turn_direction.left =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.left");
+  ip.stuck_vehicle.yield_stuck_turn_direction.right =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.right");
+  ip.stuck_vehicle.yield_stuck_turn_direction.straight =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.straight");
+  ip.stuck_vehicle.yield_stuck_distance_thr =
+    getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.yield_stuck.distance_thr");
 
   ip.collision_detection.min_predicted_path_confidence =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.min_predicted_path_confidence");
@@ -121,8 +129,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
       ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start");
   ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter<double>(
     node, ns + ".collision_detection.yield_on_green_traffic_light.duration");
-  ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.yield_on_green_traffic_light.range");
+  ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline");
+  ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration");
 
   ip.occlusion.enable = getOrDeclareParameter<bool>(node, ns + ".occlusion.enable");
   ip.occlusion.occlusion_attention_area_length =

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -67,7 +67,8 @@ class IntersectionModule : public SceneModuleInterface
         bool left;
         bool right;
         bool straight;
-      } turn_direction;
+      };
+      TurnDirection 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
@@ -78,6 +79,8 @@ class IntersectionModule : public SceneModuleInterface
       */
       double timeout_private_area;
       bool enable_private_area_stuck_disregard;
+      double yield_stuck_distance_thr;
+      TurnDirection yield_stuck_turn_direction;
     } stuck_vehicle;
     struct CollisionDetection
     {
@@ -107,8 +110,12 @@ class IntersectionModule : public SceneModuleInterface
       {
         double distance_to_assigned_lanelet_start;
         double duration;
-        double range;
+        double object_dist_to_stopline;
       } yield_on_green_traffic_light;
+      struct IgnoreOnAmberTrafficLight
+      {
+        double object_expected_deceleration;
+      } ignore_on_amber_traffic_light;
     } collision_detection;
     struct Occlusion
     {
@@ -148,6 +155,11 @@ class IntersectionModule : public SceneModuleInterface
     size_t stuck_stop_line_idx{0};
     std::optional<size_t> occlusion_stop_line_idx{std::nullopt};
   };
+  struct YieldStuckStop
+  {
+    size_t closest_idx{0};
+    size_t stuck_stop_line_idx{0};
+  };
   struct NonOccludedCollisionStop
   {
     size_t closest_idx{0};
@@ -207,6 +219,7 @@ class IntersectionModule : public SceneModuleInterface
   using DecisionResult = std::variant<
     Indecisive,                   // internal process error, or over the pass judge line
     StuckStop,                    // detected stuck vehicle
+    YieldStuckStop,               // detected yield stuck vehicle
     NonOccludedCollisionStop,     // detected collision while FOV is clear
     FirstWaitBeforeOcclusion,     // stop for a while before peeking to occlusion
     PeekingTowardOcclusion,       // peeking into occlusion while collision is not detected
@@ -258,7 +271,8 @@ class IntersectionModule : public SceneModuleInterface
 
   // for occlusion detection
   const bool enable_occlusion_detection_;
-  std::optional<std::vector<util::DiscretizedLane>> occlusion_attention_divisions_{std::nullopt};
+  std::optional<std::vector<lanelet::ConstLineString3d>> occlusion_attention_divisions_{
+    std::nullopt};
   StateMachine collision_state_machine_;     //! for stable collision checking
   StateMachine before_creep_state_machine_;  //! for two phase stop
   StateMachine occlusion_stop_state_machine_;
@@ -289,27 +303,29 @@ class IntersectionModule : public SceneModuleInterface
     const std::shared_ptr<const PlannerData> & planner_data,
     const util::PathLanelets & path_lanelets);
 
-  autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects(
-    const lanelet::ConstLanelets & attention_area_lanelets,
-    const lanelet::ConstLanelets & adjacent_lanelets,
+  bool checkYieldStuckVehicle(
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const util::PathLanelets & path_lanelets,
+    const std::optional<lanelet::CompoundPolygon3d> & first_attention_area);
+
+  util::TargetObjects generateTargetObjects(
+    const util::IntersectionLanelets & intersection_lanelets,
     const std::optional<Polygon2d> & intersection_area) const;
 
   bool checkCollision(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-    const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects,
-    const util::PathLanelets & path_lanelets, const size_t closest_idx,
-    const size_t last_intersection_stop_line_candidate_idx, const double time_delay,
-    const util::TrafficPrioritizedLevel & traffic_prioritized_level);
+    util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
+    const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
+    const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level);
 
   bool isOcclusionCleared(
     const nav_msgs::msg::OccupancyGrid & occ_grid,
     const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
     const lanelet::ConstLanelets & adjacent_lanelets,
     const lanelet::CompoundPolygon3d & first_attention_area,
     const util::InterpolatedPathInfo & interpolated_path_info,
-    const std::vector<util::DiscretizedLane> & lane_divisions,
-    const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
-      parked_attention_objects,
+    const std::vector<lanelet::ConstLineString3d> & lane_divisions,
+    const std::vector<util::TargetObject> & blocking_attention_objects,
     const double occlusion_dist_thr);
 
   /*