Merge pull request #1602 from tier4/feat/intersection-RT33896-2

fix(intersection): handle pass judge after red/arrow-signal to ignore NPCs after the signal changed to green again

Author: soblin

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1035,19 +1035,20 @@ bool CrosswalkModule::isRedSignalForPedestrians() const
     crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>();
 
   for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) {
-    const auto traffic_signal_stamped =
+    const auto traffic_signal_stamped_opt =
       planner_data_->getTrafficSignal(traffic_lights_reg_elem->id());
-    if (!traffic_signal_stamped) {
+    if (!traffic_signal_stamped_opt) {
       continue;
     }
+    const auto traffic_signal_stamped = traffic_signal_stamped_opt.value();
 
     if (
       planner_param_.traffic_light_state_timeout <
-      (clock_->now() - traffic_signal_stamped->stamp).seconds()) {
+      (clock_->now() - traffic_signal_stamped.stamp).seconds()) {
       continue;
     }
 
-    const auto & lights = traffic_signal_stamped->signal.elements;
+    const auto & lights = traffic_signal_stamped.signal.elements;
     if (lights.empty()) {
       continue;
     }

planning/behavior_velocity_intersection_module/CMakeLists.txt

@@ -8,11 +8,18 @@ pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 find_package(OpenCV REQUIRED)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/debug.cpp
   src/manager.cpp
+  src/util.cpp
   src/scene_intersection.cpp
+  src/intersection_lanelets.cpp
+  src/object_manager.cpp
+  src/decision_result.cpp
+  src/scene_intersection_prepare_data.cpp
+  src/scene_intersection_stuck.cpp
+  src/scene_intersection_occlusion.cpp
+  src/scene_intersection_collision.cpp
   src/scene_merge_from_private_road.cpp
-  src/util.cpp
+  src/debug.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

planning/behavior_velocity_intersection_module/README.md

@@ -12,6 +12,7 @@
         max_accel: -2.8
         max_jerk: -5.0
         delay_response_time: 0.5
+        enable_pass_judge_before_default_stopline: false
 
       stuck_vehicle:
         turn_direction:
@@ -23,8 +24,7 @@
         stuck_vehicle_velocity_threshold: 0.833
         # enable_front_car_decel_prediction: false
         # assumed_front_car_decel: 1.0
-        timeout_private_area: 3.0
-        enable_private_area_stuck_disregard: false
+        disable_against_private_lane: true
 
       yield_stuck:
         turn_direction:
@@ -37,7 +37,6 @@
         consider_wrong_direction_vehicle: false
         collision_detection_hold_time: 0.5
         min_predicted_path_confidence: 0.05
-        keep_detection_velocity_threshold: 0.833
         velocity_profile:
           use_upstream: true
           minimum_upstream_velocity: 0.01
@@ -60,6 +59,8 @@
           object_expected_deceleration: 2.0
         ignore_on_red_traffic_light:
           object_margin_to_path: 2.0
+        avoid_collision_by_acceleration:
+          object_time_margin_to_collision_point: 4.0
 
       occlusion:
         enable: false
@@ -73,7 +74,7 @@
           enable: false
           creep_velocity: 0.8333
         peeking_offset: -0.5
-        occlusion_required_clearance_distance: 55
+        occlusion_required_clearance_distance: 55.0
         possible_object_bbox: [1.5, 2.5]
         ignore_parked_vehicle_speed_threshold: 0.8333
         occlusion_detection_hold_time: 1.5

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -22,18 +22,19 @@
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>behavior_velocity_planner_common</depend>
+  <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>libopencv-dev</depend>
+  <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>route_handler</depend>
   <depend>rtc_interface</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>tier4_api_msgs</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>vehicle_info_util</depend>

planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg

@@ -21,6 +21,7 @@
 from threading import Lock
 import time
 
+from PIL import Image
 import imageio
 import matplotlib
 import matplotlib.pyplot as plt
@@ -218,7 +219,19 @@ def plot_world(self):
     def cleanup(self):
         if self.args.save:
             kwargs_write = {"fps": self.args.fps, "quantizer": "nq"}
-            imageio.mimsave("./" + self.args.gif + ".gif", self.images, **kwargs_write)
+            max_size_total = 0
+            max_size = None
+            for image in self.images:
+                (w, h) = image.size
+                if w * h > max_size_total:
+                    max_size = image.size
+                    max_size_total = w * h
+            reshaped = []
+            for image in self.images:
+                reshaped.append(image.resize(max_size))
+
+            imageio.mimsave("./" + self.args.gif + ".gif", reshaped, **kwargs_write)
+            print("saved fig")
         rclpy.shutdown()
 
     def on_plot_timer(self):
@@ -241,6 +254,7 @@ def on_plot_timer(self):
             if self.args.save:
                 image = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype="uint8")
                 image = image.reshape(self.fig.canvas.get_width_height()[::-1] + (3,))
+                image = Image.fromarray(image.astype(np.uint8))
                 self.images.append(image)
 
     def on_ego_ttc(self, msg):