feat(intersection): ignore decelerating vehicle on amber traffic light (

#5304) * merge attention area lanelets topologically Signed-off-by: Mamoru Sobue <[email protected]> * query stop line for each merged detection lanelet Signed-off-by: Mamoru Sobue <[email protected]> * first conflicting/attention area Signed-off-by: Mamoru Sobue <[email protected]> * ignore expectedToStopBeforeStopLine vehicle Signed-off-by: Mamoru Sobue <[email protected]> * ignore expectedToStopBeforeStopLine vehicle Signed-off-by: Mamoru Sobue <[email protected]> * fix Signed-off-by: Mamoru Sobue <[email protected]> --------- Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Oct 15, 2023 · d1a77d5 · d1a77d5
1 parent 4383d6b
commit d1a77d5
Show file tree

Hide file tree

Showing 8 changed files with 455 additions and 314 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -48,9 +48,11 @@
         use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
         minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity
         yield_on_green_traffic_light:
-          distance_to_assigned_lanelet_start: 10.0
-          duration: 3.0
-          range: 50.0 # [m]
+          distance_to_assigned_lanelet_start: 5.0
+          duration: 2.0
+          object_dist_to_stopline: 10.0 # [m]
+        ignore_on_amber_traffic_light:
+          object_expected_deceleration: 2.0 # [m/ss]
 
       occlusion:
         enable: false

diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp
@@ -233,6 +233,11 @@ 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),

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -129,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 =

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -109,8 +109,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
     {
@@ -266,7 +270,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_;
@@ -302,27 +307,24 @@ class IntersectionModule : public SceneModuleInterface
     const util::PathLanelets & path_lanelets,
     const std::optional<lanelet::CompoundPolygon3d> & first_attention_area);
 
-  autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects(
-    const lanelet::ConstLanelets & attention_area_lanelets,
-    const lanelet::ConstLanelets & adjacent_lanelets,
+  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);
 
   /*