From f2186681f2ddc2a6d70a4e9885230271c6b6846f Mon Sep 17 00:00:00 2001
From: Mingyu Li <mingyu.li@tier4.jp>
Date: Wed, 5 Jul 2023 16:52:33 +0900
Subject: [PATCH 1/9] first commit

Signed-off-by: Mingyu Li <mingyu.li@tier4.jp>
---
 .../traffic_light.launch.xml                  | 73 ++++++++++++++-----
 .../traffic_light_node_container.launch.py    | 23 ++++--
 2 files changed, 70 insertions(+), 26 deletions(-)

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 ea50101719d65..3e891ba1cfb1b 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
@@ -3,13 +3,18 @@
   <arg name="enable_image_decompressor" default="true" description="enable image decompressor"/>
   <arg name="enable_fine_detection" default="true" description="enable fine position adjustment of traffic light"/>
   <arg name="fine_detector_label_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_labels.txt" description="fine detector label path"/>
-  <arg name="fine_detector_model_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_yolox_s.onnx" description="fine detector onnx model path"/>
+  <arg name="input/cloud" default="/sensing/lidar/top/pointcloud_raw" description="point cloud for occlusion prediction"/>
+  <arg name="fine_detector_model_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_yolox_s_batch_6.onnx" description="fine detector onnx model path"/>
   <arg name="classifier_label_path" default="$(find-pkg-share traffic_light_classifier)/data/lamp_labels.txt" description="classifier label path"/>
-  <arg name="classifier_model_path" default="$(find-pkg-share traffic_light_classifier)/data/traffic_light_classifier_efficientNet_b1.onnx" description="classifier onnx model path"/>
+  <arg name="classifier_model_path" default="$(find-pkg-share traffic_light_classifier)/data/traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model path"/>
   <arg name="output/traffic_signals" default="/perception/traffic_light_recognition/traffic_signals"/>
-  <arg name="use_crosswalk_traffic_light_estimator" default="true" description="output pedestrian's traffic light signals"/>
+  <arg name="use_crosswalk_traffic_light_estimator" default="false" description="output pedestrian's traffic light signals"/>
   <let name="namespace1" value="camera6"/>
   <let name="namespace2" value="camera7"/>
+  <let name="output/rois1" value="/perception/traffic_light_recognition/$(var namespace1)/rois"/>
+  <let name="output/rois2" value="/perception/traffic_light_recognition/$(var namespace2)/rois"/>
+  <let name="output/traffic_signals1" value="/perception/traffic_light_recognition/$(var namespace1)/traffic_signals"/>
+  <let name="output/traffic_signals2" value="/perception/traffic_light_recognition/$(var namespace2)/traffic_signals"/>
   <let name="all_camera_namespaces" value="[$(var namespace1), $(var namespace2)]"/>
   <!-- camera 6 TLR pipeline -->
   <group>
@@ -17,16 +22,19 @@
     <let name="input/image" value="/sensing/camera/$(var namespace1)/image_raw"/>
     <let name="input/camera_info" value="/sensing/camera/$(var namespace1)/camera_info"/>
     <let name="map_based_detector_output_topic" value="rough/rois" if="$(var enable_fine_detection)"/>
-    <let name="map_based_detector_output_topic" value="rois" unless="$(var enable_fine_detection)"/>
+    <let name="map_based_detector_output_topic" value="$(var output/rois1)" unless="$(var enable_fine_detection)"/>
 
     <node pkg="topic_tools" exec="relay" name="traffic_light_camra_info_relay" args="$(var input/camera_info) camera_info"/>
-    <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
-      <arg name="input/camera_info" value="$(var input/camera_info)"/>
-      <arg name="min_timestamp_offset" value="-0.3"/>
-      <arg name="expect/rois" value="expect/rois"/>
-      <arg name="output/rois" value="$(var map_based_detector_output_topic)"/>
-      <arg name="output/camera_info" value="camera_info"/>
-    </include>
+    <group>
+      <push-ros-namespace namespace="detection"/>
+      <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
+        <arg name="input/camera_info" value="$(var input/camera_info)"/>
+        <arg name="min_timestamp_offset" value="-0.3"/>
+        <arg name="expect/rois" value="expect/rois"/>
+        <arg name="output/rois" value="$(var map_based_detector_output_topic)"/>
+        <arg name="output/camera_info" value="camera_info"/>
+      </include>
+    </group>
 
     <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_node_container.launch.py">
       <arg name="input/image" value="$(var input/image)"/>
@@ -39,7 +47,20 @@
       <arg name="fine_detector_model_path" value="$(var fine_detector_model_path)"/>
       <arg name="classifier_label_path" value="$(var classifier_label_path)"/>
       <arg name="classifier_model_path" value="$(var classifier_model_path)"/>
+      <arg name="output/rois" value="$(var output/rois1)"/>
+      <arg name="output/traffic_signals" value="$(var output/traffic_signals1)"/>
     </include>
+
+    <group>
+      <push-ros-namespace namespace="classification"/>
+      <include file="$(find-pkg-share traffic_light_occlusion_predictor)/launch/traffic_light_occlusion_predictor.launch.xml">
+        <arg name="input/camera_info" value="$(var input/camera_info)"/>
+        <arg name="input/cloud" value="$(var input/cloud)"/>
+        <arg name="input/rois" value="$(var output/rois1)"/>
+        <arg name="input/traffic_signals" value="estimated/traffic_signals"/>
+        <arg name="output/traffic_signals" value="$(var output/traffic_signals1)"/>
+      </include>
+    </group>
   </group>
   <!-- camera 7 TLR pipeline -->
   <group>
@@ -50,13 +71,16 @@
     <let name="map_based_detector_output_topic" value="rois" unless="$(var enable_fine_detection)"/>
 
     <node pkg="topic_tools" exec="relay" name="traffic_light_camra_info_relay" args="$(var input/camera_info) camera_info"/>
-    <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
-      <arg name="input/camera_info" value="$(var input/camera_info)"/>
-      <arg name="min_timestamp_offset" value="-0.04"/>
-      <arg name="expect/rois" value="expect/rois"/>
-      <arg name="output/rois" value="$(var map_based_detector_output_topic)"/>
-      <arg name="output/camera_info" value="camera_info"/>
-    </include>
+    <group>
+      <push-ros-namespace namespace="detection"/>
+      <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
+        <arg name="input/camera_info" value="$(var input/camera_info)"/>
+        <arg name="min_timestamp_offset" value="-0.04"/>
+        <arg name="expect/rois" value="expect/rois"/>
+        <arg name="output/rois" value="$(var map_based_detector_output_topic)"/>
+        <arg name="output/camera_info" value="camera_info"/>
+      </include>
+    </group>
 
     <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_node_container.launch.py">
       <arg name="input/image" value="$(var input/image)"/>
@@ -69,7 +93,20 @@
       <arg name="fine_detector_model_path" value="$(var fine_detector_model_path)"/>
       <arg name="classifier_label_path" value="$(var classifier_label_path)"/>
       <arg name="classifier_model_path" value="$(var classifier_model_path)"/>
+      <arg name="output/rois" value="$(var output/rois2)"/>
+      <arg name="output/traffic_signals" value="$(var output/traffic_signals2)"/>
     </include>
+
+    <group>
+      <push-ros-namespace namespace="classification"/>
+      <include file="$(find-pkg-share traffic_light_occlusion_predictor)/launch/traffic_light_occlusion_predictor.launch.xml">
+        <arg name="input/camera_info" value="$(var input/camera_info)"/>
+        <arg name="input/cloud" value="$(var input/cloud)"/>
+        <arg name="input/rois" value="$(var output/rois2)"/>
+        <arg name="input/traffic_signals" value="estimated/traffic_signals"/>
+        <arg name="output/traffic_signals" value="$(var output/traffic_signals2)"/>
+      </include>
+    </group>
   </group>
   <!-- traffic_light_multi_camera_fusion -->
   <group>
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 964008dafa5cd..dcbca9edb3723 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
@@ -40,6 +40,10 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("enable_image_decompressor", "True")
     add_launch_arg("enable_fine_detection", "True")
     add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw")
+    add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois")
+    add_launch_arg(
+        "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals"
+    )
 
     # traffic_light_fine_detector
     add_launch_arg(
@@ -89,6 +93,7 @@ def create_parameter_dict(*args):
                 package="traffic_light_classifier",
                 plugin="traffic_light::TrafficLightClassifierNodelet",
                 name="traffic_light_classifier",
+                namespace="classification",
                 parameters=[
                     create_parameter_dict(
                         "approximate_sync",
@@ -102,7 +107,7 @@ def create_parameter_dict(*args):
                 ],
                 remappings=[
                     ("~/input/image", LaunchConfiguration("input/image")),
-                    ("~/input/rois", "rois"),
+                    ("~/input/rois", LaunchConfiguration("output/rois")),
                     ("~/output/traffic_signals", "classified/traffic_signals"),
                 ],
                 extra_arguments=[
@@ -116,9 +121,9 @@ def create_parameter_dict(*args):
                 parameters=[create_parameter_dict("enable_fine_detection")],
                 remappings=[
                     ("~/input/image", LaunchConfiguration("input/image")),
-                    ("~/input/rois", "rois"),
-                    ("~/input/rough/rois", "rough/rois"),
-                    ("~/input/traffic_signals", "traffic_signals"),
+                    ("~/input/rois", LaunchConfiguration("output/rois")),
+                    ("~/input/rough/rois", "detection/rough/rois"),
+                    ("~/input/traffic_signals", LaunchConfiguration("output/traffic_signals")),
                     ("~/output/image", "debug/rois"),
                     ("~/output/image/compressed", "debug/rois/compressed"),
                     ("~/output/image/compressedDepth", "debug/rois/compressedDepth"),
@@ -138,11 +143,12 @@ def create_parameter_dict(*args):
                 package="crosswalk_traffic_light_estimator",
                 plugin="traffic_light::CrosswalkTrafficLightEstimatorNode",
                 name="crosswalk_traffic_light_estimator",
+                namespace="classification",
                 remappings=[
                     ("~/input/vector_map", "/map/vector_map"),
                     ("~/input/route", "/planning/mission_planning/route"),
                     ("~/input/classified/traffic_signals", "classified/traffic_signals"),
-                    ("~/output/traffic_signals", "traffic_signals"),
+                    ("~/output/traffic_signals", "estimated/traffic_signals"),
                 ],
                 extra_arguments=[{"use_intra_process_comms": False}],
             ),
@@ -157,10 +163,10 @@ def create_parameter_dict(*args):
                 package="topic_tools",
                 plugin="topic_tools::RelayNode",
                 name="classified_signals_relay",
-                namespace="",
+                namespace="classification",
                 parameters=[
                     {"input_topic": "classified/traffic_signals"},
-                    {"output_topic": "traffic_signals"},
+                    {"output_topic": "estimated/traffic_signals"},
                     {"type": "autoware_auto_perception_msgs/msg/TrafficSignalArray"},
                 ],
                 extra_arguments=[
@@ -209,12 +215,13 @@ def create_parameter_dict(*args):
                 package="traffic_light_fine_detector",
                 plugin="traffic_light::TrafficLightFineDetectorNodelet",
                 name="traffic_light_fine_detector",
+                namespace="detection",
                 parameters=[fine_detector_param],
                 remappings=[
                     ("~/input/image", LaunchConfiguration("input/image")),
                     ("~/input/rois", "rough/rois"),
                     ("~/expect/rois", "expect/rois"),
-                    ("~/output/rois", "rois"),
+                    ("~/output/rois", LaunchConfiguration("output/rois")),
                 ],
                 extra_arguments=[
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}

From df59b8b7fe1e9e94177334b43918d1aa013e72b3 Mon Sep 17 00:00:00 2001
From: Shunsuke Miura <shunsuke.miura@tier4.jp>
Date: Fri, 7 Jul 2023 21:28:02 +0900
Subject: [PATCH 2/9] add image number arg

Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>
---
 .../traffic_light_recognition/traffic_light.launch.xml    | 8 +++++---
 launch/tier4_perception_launch/package.xml                | 5 ++++-
 2 files changed, 9 insertions(+), 4 deletions(-)

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 ea50101719d65..a8e7004216098 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
@@ -8,10 +8,12 @@
   <arg name="classifier_model_path" default="$(find-pkg-share traffic_light_classifier)/data/traffic_light_classifier_efficientNet_b1.onnx" description="classifier onnx model path"/>
   <arg name="output/traffic_signals" default="/perception/traffic_light_recognition/traffic_signals"/>
   <arg name="use_crosswalk_traffic_light_estimator" default="true" description="output pedestrian's traffic light signals"/>
+  <arg name="image_number" default="1" description="choose image raw number(1-2)"/>
   <let name="namespace1" value="camera6"/>
   <let name="namespace2" value="camera7"/>
-  <let name="all_camera_namespaces" value="[$(var namespace1), $(var namespace2)]"/>
-  <!-- camera 6 TLR pipeline -->
+  <let name="all_camera_namespaces" value="[$(var namespace1)]" if="$(eval &quot; '$(var image_number)' == '1' &quot;)"/>
+  <let name="all_camera_namespaces" value="[$(var namespace1), $(var namespace2)]" if="$(eval &quot; '$(var image_number)' >= '2' &quot;)"/>
+  <!-- namespace1 camera TLR pipeline -->
   <group>
     <push-ros-namespace namespace="$(var namespace1)"/>
     <let name="input/image" value="/sensing/camera/$(var namespace1)/image_raw"/>
@@ -41,7 +43,7 @@
       <arg name="classifier_model_path" value="$(var classifier_model_path)"/>
     </include>
   </group>
-  <!-- camera 7 TLR pipeline -->
+  <!-- namespace2 camera TLR pipeline -->
   <group>
     <push-ros-namespace namespace="$(var namespace2)"/>
     <let name="input/image" value="/sensing/camera/$(var namespace2)/image_raw"/>
diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml
index 1140d643a7083..87aee67f34e7c 100644
--- a/launch/tier4_perception_launch/package.xml
+++ b/launch/tier4_perception_launch/package.xml
@@ -33,9 +33,12 @@
   <exec_depend>shape_estimation</exec_depend>
   <exec_depend>tensorrt_yolo</exec_depend>
   <exec_depend>traffic_light_classifier</exec_depend>
+  <exec_depend>traffic_light_fine_detector</exec_depend>
   <exec_depend>traffic_light_map_based_detector</exec_depend>
-  <exec_depend>traffic_light_ssd_fine_detector</exec_depend>
+  <exec_depend>traffic_light_multi_camera_fusion</exec_depend>
+  <exec_depend>traffic_light_occlusion_predictor</exec_depend>
   <exec_depend>traffic_light_visualization</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

From f02f05b489da52f2befe7910625cdf120f5f0695 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Fri, 7 Jul 2023 12:30:44 +0000
Subject: [PATCH 3/9] style(pre-commit): autofix

---
 launch/tier4_perception_launch/package.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml
index 87aee67f34e7c..40fc632a33f45 100644
--- a/launch/tier4_perception_launch/package.xml
+++ b/launch/tier4_perception_launch/package.xml
@@ -32,13 +32,13 @@
   <exec_depend>probabilistic_occupancy_grid_map</exec_depend>
   <exec_depend>shape_estimation</exec_depend>
   <exec_depend>tensorrt_yolo</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
   <exec_depend>traffic_light_classifier</exec_depend>
   <exec_depend>traffic_light_fine_detector</exec_depend>
   <exec_depend>traffic_light_map_based_detector</exec_depend>
   <exec_depend>traffic_light_multi_camera_fusion</exec_depend>
   <exec_depend>traffic_light_occlusion_predictor</exec_depend>
   <exec_depend>traffic_light_visualization</exec_depend>
-  <exec_depend>topic_tools</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

From adf4699c58ee44707d7dd8425566e7a27eb4d914 Mon Sep 17 00:00:00 2001
From: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
Date: Sat, 8 Jul 2023 00:24:00 +0900
Subject: [PATCH 4/9] Update
 launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml

---
 .../launch/traffic_light_recognition/traffic_light.launch.xml   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 3e891ba1cfb1b..d74b61657e36a 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
@@ -8,7 +8,7 @@
   <arg name="classifier_label_path" default="$(find-pkg-share traffic_light_classifier)/data/lamp_labels.txt" description="classifier label path"/>
   <arg name="classifier_model_path" default="$(find-pkg-share traffic_light_classifier)/data/traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model path"/>
   <arg name="output/traffic_signals" default="/perception/traffic_light_recognition/traffic_signals"/>
-  <arg name="use_crosswalk_traffic_light_estimator" default="false" description="output pedestrian's traffic light signals"/>
+  <arg name="use_crosswalk_traffic_light_estimator" default="true" description="output pedestrian's traffic light signals"/>
   <let name="namespace1" value="camera6"/>
   <let name="namespace2" value="camera7"/>
   <let name="output/rois1" value="/perception/traffic_light_recognition/$(var namespace1)/rois"/>

From f48c85775e2f1197f58c75936eee0177df723c9c Mon Sep 17 00:00:00 2001
From: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
Date: Sat, 8 Jul 2023 00:24:07 +0900
Subject: [PATCH 5/9] Update
 launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml

---
 .../launch/traffic_light_recognition/traffic_light.launch.xml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

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 d74b61657e36a..0657f497cdb8c 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
@@ -11,8 +11,8 @@
   <arg name="use_crosswalk_traffic_light_estimator" default="true" description="output pedestrian's traffic light signals"/>
   <let name="namespace1" value="camera6"/>
   <let name="namespace2" value="camera7"/>
-  <let name="output/rois1" value="/perception/traffic_light_recognition/$(var namespace1)/rois"/>
-  <let name="output/rois2" value="/perception/traffic_light_recognition/$(var namespace2)/rois"/>
+  <let name="output/rois1" value="/perception/traffic_light_recognition/$(var namespace1)/detection/rois"/>
+  <let name="output/rois2" value="/perception/traffic_light_recognition/$(var namespace2)/detection/rois"/>
   <let name="output/traffic_signals1" value="/perception/traffic_light_recognition/$(var namespace1)/traffic_signals"/>
   <let name="output/traffic_signals2" value="/perception/traffic_light_recognition/$(var namespace2)/traffic_signals"/>
   <let name="all_camera_namespaces" value="[$(var namespace1), $(var namespace2)]"/>

From 2a535a87b21c758419cad685fd2abc3cac625513 Mon Sep 17 00:00:00 2001
From: Shunsuke Miura <shunsuke.miura@tier4.jp>
Date: Sat, 8 Jul 2023 01:51:40 +0900
Subject: [PATCH 6/9] add traffic light namespace to fusion

Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>
---
 perception/traffic_light_multi_camera_fusion/src/node.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp
index d7e82af4af079..892b6be62ef9d 100644
--- a/perception/traffic_light_multi_camera_fusion/src/node.cpp
+++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp
@@ -149,8 +149,8 @@ MultiCameraFusion::MultiCameraFusion(const rclcpp::NodeOptions & node_options)
   is_approximate_sync_ = this->declare_parameter<bool>("approximate_sync", false);
   message_lifespan_ = this->declare_parameter<double>("message_lifespan", 0.09);
   for (const std::string & camera_ns : camera_namespaces) {
-    std::string signal_topic = camera_ns + "/traffic_signals";
-    std::string roi_topic = camera_ns + "/rois";
+    std::string signal_topic = camera_ns + "/classification/traffic_signals";
+    std::string roi_topic = camera_ns + "/detection/rois";
     std::string cam_info_topic = camera_ns + "/camera_info";
     roi_subs_.emplace_back(
       new mf::Subscriber<RoiArrayType>(this, roi_topic, rclcpp::QoS{1}.get_rmw_qos_profile()));

From 325d258066dae97e1cc0eae3e74c5cc609e237ac Mon Sep 17 00:00:00 2001
From: Shunsuke Miura <shunsuke.miura@tier4.jp>
Date: Sat, 8 Jul 2023 03:53:20 +0900
Subject: [PATCH 7/9] add tlr fusion only mode and camera number arg

Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>
---
 .../launch/perception.launch.xml              |   4 +
 .../traffic_light.launch.xml                  | 106 +++++++++---------
 2 files changed, 59 insertions(+), 51 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 131388b3764b5..69d42b8509bb5 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -52,6 +52,8 @@
   <!-- Traffic Light Recognition Parameters -->
   <arg name="use_traffic_light_recognition" default="true"/>
   <arg name="traffic_light_recognition/enable_fine_detection" default="true"/>
+  <arg name="traffic_light_recognition/fusion_only" default="false"/>
+  <arg name="traffic_light_image_number" default="1" description="choose traffic light image raw number(1-2)"/>
   <!-- Camera Lidar Fusion Parameters -->
   <arg name="remove_unknown" default="true"/>
   <arg name="trust_distance" default="30.0"/>
@@ -153,6 +155,8 @@
       <push-ros-namespace namespace="traffic_light_recognition"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml">
         <arg name="enable_fine_detection" value="$(var traffic_light_recognition/enable_fine_detection)"/>
+        <arg name="fusion_only" value="$(var traffic_light_recognition/fusion_only)"/>
+        <arg name="image_number" value="$(var traffic_light_image_number)"/>
       </include>
     </group>
   </group>
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 2b430aae267dd..4cf272812b064 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
@@ -2,6 +2,7 @@
 <launch>
   <arg name="enable_image_decompressor" default="true" description="enable image decompressor"/>
   <arg name="enable_fine_detection" default="true" description="enable fine position adjustment of traffic light"/>
+  <arg name="fusion_only" default="false" description="launch only occlusion_predictor and multi_camera_fusion"/>
   <arg name="fine_detector_label_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_labels.txt" description="fine detector label path"/>
   <arg name="fine_detector_model_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_yolox_s_batch_6.onnx" description="fine detector onnx model path"/>
   <arg name="classifier_label_path" default="$(find-pkg-share traffic_light_classifier)/data/lamp_labels.txt" description="classifier label path"/>
@@ -12,7 +13,6 @@
   <arg name="image_number" default="1" description="choose image raw number(1-2)"/>
   <arg name="namespace1" default="camera6"/>
   <arg name="namespace2" default="camera7"/>
-
   <let name="all_camera_namespaces" value="[$(var namespace1)]" if="$(eval &quot; '$(var image_number)' == '1' &quot;)"/>
   <let name="all_camera_namespaces" value="[$(var namespace1), $(var namespace2)]" if="$(eval &quot; '$(var image_number)' >= '2' &quot;)"/>
   <!-- namespace1 camera TLR pipeline -->
@@ -25,33 +25,35 @@
     <let name="map_based_detector_output_topic" value="rough/rois" if="$(var enable_fine_detection)"/>
     <let name="map_based_detector_output_topic" value="$(var output/rois1)" unless="$(var enable_fine_detection)"/>
 
-    <node pkg="topic_tools" exec="relay" name="traffic_light_camra_info_relay" args="$(var input/camera_info) camera_info"/>
-    <group>
-      <push-ros-namespace namespace="detection"/>
-      <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
-        <arg name="input/camera_info" value="$(var input/camera_info)"/>
-        <arg name="min_timestamp_offset" value="-0.3"/>
-        <arg name="expect/rois" value="expect/rois"/>
-        <arg name="output/rois" value="$(var map_based_detector_output_topic)"/>
-        <arg name="output/camera_info" value="camera_info"/>
+    <group unless="$(var fusion_only)">
+      <node pkg="topic_tools" exec="relay" name="traffic_light_camra_info_relay" args="$(var input/camera_info) camera_info"/>
+      <group>
+        <push-ros-namespace namespace="detection"/>
+        <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
+          <arg name="input/camera_info" value="$(var input/camera_info)"/>
+          <arg name="min_timestamp_offset" value="-0.3"/>
+          <arg name="expect/rois" value="expect/rois"/>
+          <arg name="output/rois" value="$(var map_based_detector_output_topic)"/>
+          <arg name="output/camera_info" value="camera_info"/>
+        </include>
+      </group>
+
+      <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_node_container.launch.py">
+        <arg name="input/image" value="$(var input/image)"/>
+        <arg name="enable_image_decompressor" value="$(var enable_image_decompressor)"/>
+        <arg name="enable_fine_detection" value="$(var enable_fine_detection)"/>
+        <arg name="use_crosswalk_traffic_light_estimator" value="$(var use_crosswalk_traffic_light_estimator)"/>
+        <arg name="use_intra_process" value="true"/>
+        <arg name="use_multithread" value="true"/>
+        <arg name="fine_detector_label_path" value="$(var fine_detector_label_path)"/>
+        <arg name="fine_detector_model_path" value="$(var fine_detector_model_path)"/>
+        <arg name="classifier_label_path" value="$(var classifier_label_path)"/>
+        <arg name="classifier_model_path" value="$(var classifier_model_path)"/>
+        <arg name="output/rois" value="$(var output/rois1)"/>
+        <arg name="output/traffic_signals" value="$(var output/traffic_signals1)"/>
       </include>
     </group>
 
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_node_container.launch.py">
-      <arg name="input/image" value="$(var input/image)"/>
-      <arg name="enable_image_decompressor" value="$(var enable_image_decompressor)"/>
-      <arg name="enable_fine_detection" value="$(var enable_fine_detection)"/>
-      <arg name="use_crosswalk_traffic_light_estimator" value="$(var use_crosswalk_traffic_light_estimator)"/>
-      <arg name="use_intra_process" value="true"/>
-      <arg name="use_multithread" value="true"/>
-      <arg name="fine_detector_label_path" value="$(var fine_detector_label_path)"/>
-      <arg name="fine_detector_model_path" value="$(var fine_detector_model_path)"/>
-      <arg name="classifier_label_path" value="$(var classifier_label_path)"/>
-      <arg name="classifier_model_path" value="$(var classifier_model_path)"/>
-      <arg name="output/rois" value="$(var output/rois1)"/>
-      <arg name="output/traffic_signals" value="$(var output/traffic_signals1)"/>
-    </include>
-
     <group>
       <push-ros-namespace namespace="classification"/>
       <include file="$(find-pkg-share traffic_light_occlusion_predictor)/launch/traffic_light_occlusion_predictor.launch.xml">
@@ -64,7 +66,7 @@
     </group>
   </group>
   <!-- namespace2 camera TLR pipeline -->
-  <group>
+  <group if="$(eval &quot; '$(var image_number)' >= '2' &quot;)">
     <push-ros-namespace namespace="$(var namespace2)"/>
     <let name="input/image" value="/sensing/camera/$(var namespace2)/image_raw"/>
     <let name="input/camera_info" value="/sensing/camera/$(var namespace2)/camera_info"/>
@@ -73,33 +75,35 @@
     <let name="map_based_detector_output_topic" value="rough/rois" if="$(var enable_fine_detection)"/>
     <let name="map_based_detector_output_topic" value="rois" unless="$(var enable_fine_detection)"/>
 
-    <node pkg="topic_tools" exec="relay" name="traffic_light_camra_info_relay" args="$(var input/camera_info) camera_info"/>
-    <group>
-      <push-ros-namespace namespace="detection"/>
-      <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
-        <arg name="input/camera_info" value="$(var input/camera_info)"/>
-        <arg name="min_timestamp_offset" value="-0.04"/>
-        <arg name="expect/rois" value="expect/rois"/>
-        <arg name="output/rois" value="$(var map_based_detector_output_topic)"/>
-        <arg name="output/camera_info" value="camera_info"/>
+    <group unless="$(var fusion_only)">
+      <node pkg="topic_tools" exec="relay" name="traffic_light_camra_info_relay" args="$(var input/camera_info) camera_info"/>
+      <group>
+        <push-ros-namespace namespace="detection"/>
+        <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
+          <arg name="input/camera_info" value="$(var input/camera_info)"/>
+          <arg name="min_timestamp_offset" value="-0.04"/>
+          <arg name="expect/rois" value="expect/rois"/>
+          <arg name="output/rois" value="$(var map_based_detector_output_topic)"/>
+          <arg name="output/camera_info" value="camera_info"/>
+        </include>
+      </group>
+
+      <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_node_container.launch.py">
+        <arg name="input/image" value="$(var input/image)"/>
+        <arg name="enable_image_decompressor" value="$(var enable_image_decompressor)"/>
+        <arg name="enable_fine_detection" value="$(var enable_fine_detection)"/>
+        <arg name="use_crosswalk_traffic_light_estimator" value="$(var use_crosswalk_traffic_light_estimator)"/>
+        <arg name="use_intra_process" value="true"/>
+        <arg name="use_multithread" value="true"/>
+        <arg name="fine_detector_label_path" value="$(var fine_detector_label_path)"/>
+        <arg name="fine_detector_model_path" value="$(var fine_detector_model_path)"/>
+        <arg name="classifier_label_path" value="$(var classifier_label_path)"/>
+        <arg name="classifier_model_path" value="$(var classifier_model_path)"/>
+        <arg name="output/rois" value="$(var output/rois2)"/>
+        <arg name="output/traffic_signals" value="$(var output/traffic_signals2)"/>
       </include>
     </group>
 
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light_node_container.launch.py">
-      <arg name="input/image" value="$(var input/image)"/>
-      <arg name="enable_image_decompressor" value="$(var enable_image_decompressor)"/>
-      <arg name="enable_fine_detection" value="$(var enable_fine_detection)"/>
-      <arg name="use_crosswalk_traffic_light_estimator" value="$(var use_crosswalk_traffic_light_estimator)"/>
-      <arg name="use_intra_process" value="true"/>
-      <arg name="use_multithread" value="true"/>
-      <arg name="fine_detector_label_path" value="$(var fine_detector_label_path)"/>
-      <arg name="fine_detector_model_path" value="$(var fine_detector_model_path)"/>
-      <arg name="classifier_label_path" value="$(var classifier_label_path)"/>
-      <arg name="classifier_model_path" value="$(var classifier_model_path)"/>
-      <arg name="output/rois" value="$(var output/rois2)"/>
-      <arg name="output/traffic_signals" value="$(var output/traffic_signals2)"/>
-    </include>
-
     <group>
       <push-ros-namespace namespace="classification"/>
       <include file="$(find-pkg-share traffic_light_occlusion_predictor)/launch/traffic_light_occlusion_predictor.launch.xml">
@@ -111,7 +115,7 @@
       </include>
     </group>
   </group>
-  
+
   <!-- traffic_light_multi_camera_fusion -->
   <group>
     <node pkg="traffic_light_multi_camera_fusion" exec="traffic_light_multi_camera_fusion_node" name="traffic_light_multi_camera_fusion" output="screen">

From 17e2c504c828aecd6f65d147f656f36c26fd3464 Mon Sep 17 00:00:00 2001
From: Shunsuke Miura <shunsuke.miura@tier4.jp>
Date: Sat, 8 Jul 2023 04:15:25 +0900
Subject: [PATCH 8/9] change to include traffic_light_arbiter launch

Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>
---
 .../traffic_light.launch.xml                          | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

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 4cf272812b064..4dae46cbacb0a 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
@@ -8,6 +8,7 @@
   <arg name="classifier_label_path" default="$(find-pkg-share traffic_light_classifier)/data/lamp_labels.txt" description="classifier label path"/>
   <arg name="classifier_model_path" default="$(find-pkg-share traffic_light_classifier)/data/traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model path"/>
   <arg name="input/cloud" default="/sensing/lidar/top/pointcloud_raw" description="point cloud for occlusion prediction"/>
+  <arg name="internal/traffic_signals" default="/perception/traffic_light_recognition/internal/traffic_signals"/>
   <arg name="output/traffic_signals" default="/perception/traffic_light_recognition/traffic_signals"/>
   <arg name="use_crosswalk_traffic_light_estimator" default="true" description="output pedestrian's traffic light signals"/>
   <arg name="image_number" default="1" description="choose image raw number(1-2)"/>
@@ -122,9 +123,17 @@
       <param name="camera_namespaces" value="$(var all_camera_namespaces)"/>
       <param name="perform_group_fusion" value="true"/>
       <remap from="~/input/vector_map" to="/map/vector_map"/>
-      <remap from="~/output/traffic_signals" to="$(var output/traffic_signals)"/>
+      <remap from="~/output/traffic_signals" to="$(var internal/traffic_signals)"/>
     </node>
   </group>
+  <!-- V2X fusion -->
+  <group>
+    <include file="$(find-pkg-share traffic_light_arbiter)/launch/traffic_light_arbiter.launch.xml">
+      <arg name="perception_traffic_signals" value="$(var internal/traffic_signals)"/>
+      <arg name="external_traffic_signals" value="/external/traffic_light_recognition/traffic_signals"/>
+      <arg name="output_traffic_signals" value="$(var output/traffic_signals)"/>
+    </include>
+  </group>
   <!-- visualizer -->
   <group>
     <include file="$(find-pkg-share traffic_light_visualization)/launch/traffic_light_map_visualizer.launch.xml"/>

From 854e71e2fab380584be3fc4278a7853612039335 Mon Sep 17 00:00:00 2001
From: Shunsuke Miura <shunsuke.miura@tier4.jp>
Date: Sat, 8 Jul 2023 04:24:11 +0900
Subject: [PATCH 9/9] delete relay topic type

Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>
---
 .../traffic_light_node_container.launch.py                       | 1 -
 1 file changed, 1 deletion(-)

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 dcbca9edb3723..8b3d15f2cff95 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
@@ -167,7 +167,6 @@ def create_parameter_dict(*args):
                 parameters=[
                     {"input_topic": "classified/traffic_signals"},
                     {"output_topic": "estimated/traffic_signals"},
-                    {"type": "autoware_auto_perception_msgs/msg/TrafficSignalArray"},
                 ],
                 extra_arguments=[
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}