feat(tier4_perception_launch): update traffic light launch (autowaref…

…oundation#4176) * first commit Signed-off-by: Mingyu Li <mingyu.li@tier4.jp> * add image number arg Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * style(pre-commit): autofix * Update launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml * Update launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml * add traffic light namespace to fusion Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * add tlr fusion only mode and camera number arg Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * change to include traffic_light_arbiter launch Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * delete relay topic type Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> --------- Signed-off-by: Mingyu Li <mingyu.li@tier4.jp> Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> Co-authored-by: Shunsuke Miura <shunsuke.miura@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
kyoichi-sugahara · Jul 9, 2023 · 9d93c3b · 9d93c3b
1 parent 661cdbe
commit 9d93c3b
Show file tree

Hide file tree

Showing 5 changed files with 129 additions and 62 deletions.
diff --git 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
@@ -2,84 +2,138 @@
 <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.onnx" description="fine detector onnx model 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"/>
-  <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="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"/>
-  <let name="namespace1" value="camera6"/>
-  <let name="namespace2" value="camera7"/>
-  <let name="all_camera_namespaces" value="[$(var namespace1), $(var namespace2)]"/>
-  <!-- camera 6 TLR pipeline -->
+  <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 -->
   <group>
     <push-ros-namespace namespace="$(var namespace1)"/>
     <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="output/rois1" value="/perception/traffic_light_recognition/$(var namespace1)/detection/rois"/>
+    <let name="output/traffic_signals1" value="/perception/traffic_light_recognition/$(var namespace1)/classification/traffic_signals"/>
     <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 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)"/>
-    </include>
+      <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>
+
+    <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>
+  <!-- namespace2 camera TLR pipeline -->
+  <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"/>
+    <let name="output/rois2" value="/perception/traffic_light_recognition/$(var namespace2)/detection/rois"/>
+    <let name="output/traffic_signals2" value="/perception/traffic_light_recognition/$(var namespace2)/classification/traffic_signals"/>
     <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"/>
-    <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 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)"/>
-    </include>
+      <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>
+
+    <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>
     <node pkg="traffic_light_multi_camera_fusion" exec="traffic_light_multi_camera_fusion_node" name="traffic_light_multi_camera_fusion" output="screen">
       <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"/>

diff --git a/...perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/...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,11 +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"},
-                    {"type": "autoware_auto_perception_msgs/msg/TrafficSignalArray"},
+                    {"output_topic": "estimated/traffic_signals"},
                 ],
                 extra_arguments=[
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -209,12 +214,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")}

diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml
@@ -32,9 +32,12 @@
   <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_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>
 
   <test_depend>ament_lint_auto</test_depend>

diff --git 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()));