DO NOT MERGE feat(intersection): upgrade intersection module

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/config/intersection.param.yaml

@@ -12,8 +12,18 @@
         max_accel: -2.8
         max_jerk: -5.0
         delay_response_time: 0.5
+        enable_pass_judge_before_default_stopline: false
 
       stuck_vehicle:
+        target_type:
+          car: true
+          bus: true
+          truck: true
+          trailer: true
+          motorcycle: false
+          bicycle: false
+          unknown: false
+
         turn_direction:
           left: true
           right: true
@@ -23,10 +33,17 @@
         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:
+        target_type:
+          car: true
+          bus: true
+          truck: true
+          trailer: true
+          motorcycle: false
+          bicycle: false
+          unknown: false
         turn_direction:
           left: true
           right: true
@@ -37,7 +54,14 @@
         consider_wrong_direction_vehicle: false
         collision_detection_hold_time: 0.5
         min_predicted_path_confidence: 0.05
-        keep_detection_velocity_threshold: 0.833
+        target_type:
+          car: true
+          bus: true
+          truck: true
+          trailer: true
+          motorcycle: true
+          bicycle: true
+          unknown: false
         velocity_profile:
           use_upstream: true
           minimum_upstream_velocity: 0.01
@@ -57,9 +81,13 @@
           duration: 3.0
           object_dist_to_stopline: 10.0
         ignore_on_amber_traffic_light:
-          object_expected_deceleration: 2.0
+          object_expected_deceleration:
+            car: 2.0
+            bike: 5.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 +101,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/package.xml

@@ -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/scripts/ttc.py

@@ -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 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 @@
             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):