From e9bdc642eac80cf21acfcd701f05eaba790f723e Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Fri, 6 Dec 2024 10:03:33 +0900
Subject: [PATCH] feat: add traffic_light_detector launch

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

fix: tier4 perception launch

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

fix: add multi tlr detector launch

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

fix: tier4 launch

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

fix: tl detector launch

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

fix: data director

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

fix: precision int8

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

chore: revert to fp16

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

feat: remove occlusion and add car ped classification merger

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

fix: launch for multi camera

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

chore: pre-commit

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

fix: update matching score

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

feat: add max_iou_threshold

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

feat: add occlusion unknown classifier

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../traffic_light.launch.xml                  |  5 +-
 .../traffic_light_detector.launch.xml         | 70 +++++++++++++++++++
 ...traffic_light_map_based_detector.launch.py | 28 ++++++++
 .../traffic_light_node_container.launch.py    | 22 +++++-
 4 files changed, 121 insertions(+), 4 deletions(-)
 create mode 100644 launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml

diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
index d4a0a8b5abdd8..7735ef62ec537 100644
--- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
@@ -34,6 +34,7 @@
       <arg name="all_camera_namespaces" value="$(var all_camera_namespaces)"/>
       <arg name="enable_fine_detection" value="$(var enable_fine_detection)"/>
     </include>
+    <!-- <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_detector.launch.xml"/> -->
     <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_node_container.launch.py">
       <arg name="all_camera_namespaces" value="$(var all_camera_namespaces)"/>
       <arg name="enable_image_decompressor" value="$(var enable_image_decompressor)"/>
@@ -47,12 +48,12 @@
   </group>
 
   <!-- traffic_light_occlusion_predictor on each camera -->
-  <group>
+  <!-- <group>
     <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_occlusion_predictor.launch.py">
       <arg name="all_camera_namespaces" value="$(var all_camera_namespaces)"/>
       <arg name="input/cloud" value="$(var input/cloud)"/>
     </include>
-  </group>
+  </group> -->
 
   <!-- traffic_light_multi_camera_fusion -->
   <group>
diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml
new file mode 100644
index 0000000000000..ca41ad8b3147e
--- /dev/null
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml
@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<launch>
+  <group>
+    <!-- add namespace -->
+    <arg name="namespace" default="camera6"/>
+    <arg name="input/image" default="/sensing/camera/$(var namespace)/image_raw"/>
+    <arg name="yolox_detected_traffic/rois" default="/perception/traffic_light_recognition/$(var namespace)/detection/yolox/rois"/>
+    <arg
+      name="model_name"
+      default="tlr_car_ped_yolox_s_960_960_batch_1"
+      description="options `yolox-sPlus-T4-960x960-pseudo-finetune` if only detection is needed, `yolox-sPlus-opt-pseudoV2-T4-960x960-T4-seg16cls` if sematic segmentation is also needed"
+    />
+    <arg name="use_decompress" default="true" description="use image decompress"/>
+    <arg name="build_only" default="false" description="exit after trt engine is built"/>
+
+    <arg name="param_file" default="$(find-pkg-share autoware_image_transport_decompressor)/config/image_transport_decompressor.param.yaml"/>
+    <node pkg="autoware_image_transport_decompressor" exec="image_transport_decompressor_node" name="image_transport_decompressor_$(var namespace)_node" if="$(var use_decompress)">
+      <remap from="~/input/compressed_image" to="$(var input/image)/compressed"/>
+      <remap from="~/output/raw_image" to="$(var input/image)"/>
+      <param from="$(var param_file)"/>
+    </node>
+
+    <node pkg="autoware_tensorrt_yolox" exec="autoware_tensorrt_yolox_node_exe" name="traffic_light_yolox_$(var namespace)" output="screen">
+      <remap from="~/in/image" to="$(var input/image)"/>
+      <remap from="~/out/objects" to="$(var yolox_detected_traffic/rois)"/>
+      <param name="build_only" value="$(var build_only)"/>
+      <param name="is_roi_overlap_segment" value="false"/>
+      <param name="overlap_roi_score_threshold" value="0.3"/>
+      <param name="is_publish_color_mask" value="false"/>
+      <param name="roi_overlay_segment_label.UNKNOWN" value="false"/>
+      <param name="roi_overlay_segment_label.CAR" value="false"/>
+      <param name="roi_overlay_segment_label.TRUCK" value="false"/>
+      <param name="roi_overlay_segment_label.BUS" value="false"/>
+      <param name="roi_overlay_segment_label.MOTORCYCLE" value="false"/>
+      <param name="roi_overlay_segment_label.BICYCLE" value="false"/>
+      <param name="roi_overlay_segment_label.PEDESTRIAN" value="false"/>
+      <param name="roi_overlay_segment_label.ANIMAL" value="false"/>
+      <param name="model_path" value="/opt/autoware/mlmodels/traffic_light_detector/$(var model_name).onnx"/>
+      <param name="label_path" value="/opt/autoware/mlmodels/traffic_light_detector/label.txt"/>
+      <param name="color_map_path" value=""/>
+      <param name="score_threshold" value="0.35"/>
+      <param name="nms_threshold" value="0.7"/>
+      <param name="precision" value="fp16"/>
+      <param name="calibration_algorithm" value="Entropy"/>
+      <param name="dla_core_id" value="-1"/>
+      <param name="quantize_first_layer" value="false"/>
+      <param name="quantize_last_layer" value="false"/>
+      <param name="profile_per_layer" value="false"/>
+      <param name="clip_value" value="6.0"/>
+      <param name="preprocess_on_gpu" value="true"/>
+      <param name="gpu_id" value="0"/>
+      <param name="calibration_image_list_path" value=""/>
+    </node>
+    <arg name="input/detected_rois" default="input/detected_rois"/>
+    <arg name="input/rough_rois" default="/perception/traffic_light_recognition/$(var namespace)/detection/rough/rois"/>
+    <arg name="output/traffic_light_rois" default="/perception/traffic_light_recognition/$(var namespace)/detection/rois"/>
+    <arg name="input/expect_rois" default="/perception/traffic_light_recognition/$(var namespace)/detection/expect/rois"/>
+    <arg name="input/camera_info" default="/sensing/camera/$(var namespace)/camera_info"/>
+    <!-- Node -->
+    <node pkg="autoware_traffic_light_selector" exec="traffic_light_selector_node" name="autoware_traffic_light_selector_$(var namespace)" output="screen">
+      <remap from="input/detected_rois" to="$(var yolox_detected_traffic/rois)"/>
+      <remap from="input/rough_rois" to="$(var input/rough_rois)"/>
+      <remap from="input/expect_rois" to="$(var input/expect_rois)"/>
+      <remap from="output/traffic_light_rois" to="$(var output/traffic_light_rois)"/>
+      <remap from="input/camera_info" to="$(var input/camera_info)"/>
+      <param name="max_iou_threshold" value="0.0"/>
+      <param name="debug" value="true"/>
+    </node>
+  </group>
+</launch>
diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_map_based_detector.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_map_based_detector.launch.py
index e38c05f9caa3e..8cb961dded957 100644
--- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_map_based_detector.launch.py
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_map_based_detector.launch.py
@@ -56,6 +56,27 @@ def create_traffic_light_map_based_detector(namespace, context):
     return group
 
 
+def create_traffic_light_detector(namespace, context):
+    package = FindPackageShare("tier4_perception_launch")
+    include = PathJoinSubstitution(
+        [package, "launch/traffic_light_recognition/traffic_light_detector.launch.xml"]
+    )
+    arguments = {
+        "namespace": namespace,
+        "use_decompress": "true",
+    }.items()
+
+    group = GroupAction(
+        [
+            PushRosNamespace(namespace),
+            PushRosNamespace("detection"),
+            IncludeLaunchDescription(include, launch_arguments=arguments),
+        ]
+    )
+
+    return group
+
+
 def launch_setup(context, *args, **kwargs):
     # Load all camera namespaces
     all_camera_namespaces = LaunchConfiguration("all_camera_namespaces").perform(context)
@@ -76,6 +97,13 @@ def launch_setup(context, *args, **kwargs):
         create_traffic_light_map_based_detector(namespace, context)
         for namespace in all_camera_namespaces
     ]
+
+    _ = [
+        traffic_light_recognition_containers.append(
+            create_traffic_light_detector(namespace, context)
+        )
+        for namespace in all_camera_namespaces
+    ]
     return traffic_light_recognition_containers
 
 
diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
index 9603570b3cfe7..0aa8f8d07c743 100644
--- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
@@ -122,6 +122,24 @@ def create_parameter_dict(*args):
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
                 ],
             ),
+            ComposableNode(
+                package="autoware_traffic_light_signals_merger",
+                plugin="autoware::traffic_light::TrafficLightSignalsMergerNode",
+                name="traffic_light_signals_merger",
+                namespace="classification",
+                remappings=[
+                    ("input/car_signals", "classified/car/traffic_signals"),
+                    ("input/pedestrian_signals", "classified/pedestrian/traffic_signals"),
+                    (
+                        "input/expect_rois",
+                        f"/perception/traffic_light_recognition/{namespace}/detection/expect/rois",
+                    ),
+                    ("output/traffic_light_signals", "traffic_signals"),
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            ),
             ComposableNode(
                 package="autoware_traffic_light_visualization",
                 plugin="autoware::traffic_light::TrafficLightRoiVisualizerNode",
@@ -197,8 +215,8 @@ def create_parameter_dict(*args):
 
     return [
         GroupAction([PushRosNamespace(namespace), container]),
-        decompressor_loader,
-        fine_detector_loader,
+        # decompressor_loader,
+        # traffic_light_selector_loader,
     ]