Merge pull request autowarefoundation#106 from tier4/sync-upstream

chore: sync upstream
TomohitoAndo · Mar 3, 2022 · f2366af · f2366af
2 parents 7457083 + 92ff8e7
commit f2366af
Show file tree

Hide file tree

Showing 5 changed files with 105 additions and 22 deletions.
diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml
@@ -12,7 +12,8 @@
   <!-- Optional parameters for scenario simulation -->
   <arg name="initial_engage_state" default="true" description="/vehicle/engage state after starting Autoware"/>
   <arg name="perception/enable_detection_failure" default="true" description="enable to simulate detection failure when using dummy perception"/>
-  <arg name="perception/enable_object_recognition" default="true" description="enable object recognition when using dummy perception"/>
+  <arg name="perception/enable_object_recognition" default="true" description="enable object recognition"/>
+  <arg name="perception/enable_traffic_light" default="false" description="enable traffic light"/>
   <arg name="sensing/visible_range" default="300.0" description="visible range when using dummy perception"/>
   <arg name="scenario_simulation" default="false" description="use scenario simulation"/>
   <!-- Optional parameters for vcu emulation -->
@@ -70,6 +71,7 @@
     <arg name="scenario_simulation" value="$(var scenario_simulation)"/>
     <arg name="perception/enable_detection_failure" value="$(var perception/enable_detection_failure)"/>
     <arg name="perception/enable_object_recognition" value="$(var perception/enable_object_recognition)"/>
+    <arg name="perception/enable_traffic_light" value="$(var perception/enable_traffic_light)"/>
     <arg name="sensing/visible_range" value="$(var sensing/visible_range)"/>
     <arg name="vehicle_simulation" value="$(var vehicle_simulation)"/>
     <arg name="vehicle_model" value="$(var vehicle_model)"/>

diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
@@ -1075,6 +1075,20 @@ Visualization Manager:
                         Reliability Policy: Reliable
                         Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection
                       Value: true
+                    - Class: rviz_default_plugins/MarkerArray
+                      Enabled: true
+                      Name: MergeFromPrivate
+                      Namespaces:
+                        factor_text: true
+                        stop_point_pose_line: false
+                        stop_virtual_wall: true
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/merge_from_private
+                      Value: true
                     - Class: rviz_default_plugins/MarkerArray
                       Enabled: true
                       Name: Blind Spot

diff --git a/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
@@ -1,9 +1,22 @@
 <?xml version="1.0"?>
 
 <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="input/image" default="/sensing/camera/traffic_light/image_raw" description="image raw topic name for traffic light"/>
   <arg name="input/camera_info" default="/sensing/camera/traffic_light/camera_info" description="camera info topic name for traffic light"/>
+  <arg name="fine_detector_label_path"
+       default="$(find-pkg-share traffic_light_ssd_fine_detector)/data/voc_labels_tl.txt"
+       description="fine detector label path"/>
+  <arg name="fine_detector_model_path"
+       default="$(find-pkg-share traffic_light_ssd_fine_detector)/data/mb2-ssd-lite-tlr.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_mobilenetv2.onnx"
+       description="classifier onnx model path"/>
 
   <group if="$(var enable_fine_detection)">
     <include file="$(find-pkg-share traffic_light_map_based_detector)/launch/traffic_light_map_based_detector.launch.xml">
@@ -22,9 +35,14 @@
   <!-- classification -->
   <include file="$(find-pkg-share 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_intra_process" value="true"/>
     <arg name="use_multithread" value="false"/>
+    <arg name="label_file" value="$(var fine_detector_label_path)"/>
+    <arg name="onnx_file" value="$(var fine_detector_model_path)"/>
+    <arg name="label_file_path" value="$(var classifier_label_path)"/>
+    <arg name="model_file_path" value="$(var classifier_model_path)"/>
   </include>
 
   <!-- visualizer -->

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
@@ -15,7 +15,6 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-import launch
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.actions import SetLaunchConfiguration
@@ -38,6 +37,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
 
     ssd_fine_detector_share_dir = get_package_share_directory("traffic_light_ssd_fine_detector")
     classifier_share_dir = get_package_share_directory("traffic_light_classifier")
+    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")
 
@@ -82,22 +82,6 @@ def create_parameter_dict(*args):
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
-            ComposableNode(
-                package="image_transport_decompressor",
-                plugin="image_preprocessor::ImageTransportDecompressor",
-                name="traffic_light_image_decompressor",
-                parameters=[{"encoding": "rgb8"}],
-                remappings=[
-                    (
-                        "~/input/compressed_image",
-                        [LaunchConfiguration("input/image"), "/compressed"],
-                    ),
-                    ("~/output/raw_image", LaunchConfiguration("input/image")),
-                ],
-                extra_arguments=[
-                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
-                ],
-            ),
             ComposableNode(
                 package="traffic_light_classifier",
                 plugin="traffic_light::TrafficLightClassifierNodelet",
@@ -146,6 +130,29 @@ def create_parameter_dict(*args):
         output="both",
     )
 
+    decompressor_loader = LoadComposableNodes(
+        composable_node_descriptions=[
+            ComposableNode(
+                package="image_transport_decompressor",
+                plugin="image_preprocessor::ImageTransportDecompressor",
+                name="traffic_light_image_decompressor",
+                parameters=[{"encoding": "rgb8"}],
+                remappings=[
+                    (
+                        "~/input/compressed_image",
+                        [LaunchConfiguration("input/image"), "/compressed"],
+                    ),
+                    ("~/output/raw_image", LaunchConfiguration("input/image")),
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            ),
+        ],
+        target_container=container,
+        condition=IfCondition(LaunchConfiguration("enable_image_decompressor")),
+    )
+
     ssd_fine_detector_param = create_parameter_dict(
         "onnx_file",
         "label_file",
@@ -157,7 +164,7 @@ def create_parameter_dict(*args):
     )
     ssd_fine_detector_param["mode"] = LaunchConfiguration("fine_detector_precision")
 
-    loader = LoadComposableNodes(
+    fine_detector_loader = LoadComposableNodes(
         composable_node_descriptions=[
             ComposableNode(
                 package="traffic_light_ssd_fine_detector",
@@ -175,7 +182,7 @@ def create_parameter_dict(*args):
             ),
         ],
         target_container=container,
-        condition=launch.conditions.IfCondition(LaunchConfiguration("enable_fine_detection")),
+        condition=IfCondition(LaunchConfiguration("enable_fine_detection")),
     )
 
     set_container_executable = SetLaunchConfiguration(
@@ -196,6 +203,7 @@ def create_parameter_dict(*args):
             set_container_executable,
             set_container_mt_executable,
             container,
-            loader,
+            decompressor_loader,
+            fine_detector_loader,
         ]
     )
diff --git a/simulator_launch/launch/simulator.launch.xml b/simulator_launch/launch/simulator.launch.xml
@@ -2,7 +2,8 @@
   <arg name="scenario_simulation" default="" description="use scenario simulation"/>
 
   <arg name="perception/enable_detection_failure" default="true" description="enable to simulate detection failure when using dummy perception"/>
-  <arg name="perception/enable_object_recognition" default="false" description="enable object recognition when using dummy perception"/>
+  <arg name="perception/enable_object_recognition" default="false" description="enable object recognition"/>
+  <arg name="perception/enable_traffic_light" default="false" description="enable traffic light"/>
   <arg name="sensing/visible_range" default="300.0" description="visible range when using dummy perception"/>
 
   <arg name="vehicle_simulation" default="true" description="use vehicle simulation"/>
@@ -24,6 +25,46 @@
     </include>
   </group>
 
+  <!-- perception module -->
+  <group>
+    <push-ros-namespace namespace="perception"/>
+    <!-- object recognition -->
+    <group if="$(var perception/enable_object_recognition)">
+      <push-ros-namespace namespace="object_recognition"/>
+      <!-- tracking module -->
+      <group>
+        <push-ros-namespace namespace="tracking"/>
+        <include file="$(find-pkg-share perception_launch)/launch/object_recognition/tracking/tracking.launch.xml">
+        </include>
+      </group>
+      <!-- prediction module -->
+      <group>
+        <push-ros-namespace namespace="prediction"/>
+        <include file="$(find-pkg-share perception_launch)/launch/object_recognition/prediction/prediction.launch.xml">
+          <arg name="use_vector_map" value="true" />
+        </include>
+      </group>
+    </group>
+
+    <!-- object segmentation module -->
+    <group>
+      <push-ros-namespace namespace="occupancy_grid_map"/>
+      <!-- Occupancy Grid -->
+      <include file="$(find-pkg-share laserscan_to_occupancy_grid_map)/launch/laserscan_to_occupancy_grid_map.launch.py">
+        <arg name="input_obstacle_pointcloud" value="true" />
+        <arg name="input_obstacle_and_raw_pointcloud" value="false" />
+        <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud" />
+        <arg name="output" value="/perception/occupancy_grid_map/map" />
+      </include>
+    </group>
+
+    <!-- traffic light module -->
+    <group if="$(var perception/enable_traffic_light)">
+      <push-ros-namespace namespace="traffic_light_recognition"/>
+      <include file="$(find-pkg-share perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml"></include>
+    </group>
+  </group>
+
   <!-- Simulator model -->
   <group if="$(var vehicle_simulation)">
     <group unless="$(var scenario_simulation)">