feat(tier4_perception_launch): sync perception launch to autoware_lau…

…nch (autowarefoundation#2168) * sync launch file from tier4 autoware launch Signed-off-by: Shunsuke Miura <[email protected]> * sync tlr launcher Signed-off-by: Shunsuke Miura <[email protected]> * ci(pre-commit): autofix * sync launch file from tier4 autoware launch Signed-off-by: Shunsuke Miura <[email protected]> * sync tlr launcher Signed-off-by: Shunsuke Miura <[email protected]> * ci(pre-commit): autofix * fix exec_depend in package.xml Signed-off-by: Shunsuke Miura <[email protected]> * Sync traffic light node Signed-off-by: Shunsuke Miura <[email protected]> Signed-off-by: Shunsuke Miura <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: Kotaro Yoshimoto <[email protected]>
HansRobo · Dec 16, 2022 · e35311e · e35311e
1 parent 8f25680
commit e35311e
Show file tree

Hide file tree

Showing 7 changed files with 104 additions and 36 deletions.
diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml
@@ -7,6 +7,8 @@
   <group>
     <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
       <arg name="data_association_matrix_path" value="$(var tier4_perception_launch_param_path)/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml"/>
+      <arg name="publish_rate" value="$(var publish_rate)"/>
+      <arg name="enable_delay_compensation" value="$(var enable_delay_compensation)"/>
     </include>
   </group>
 </launch>
diff --git a/...ion_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/...ion_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
@@ -47,15 +47,20 @@ def __init__(self, context):
         self.use_time_series_filter = self.ground_segmentation_param["use_time_series_filter"]
 
     def get_vehicle_info(self):
-        path = LaunchConfiguration("vehicle_param_file").perform(self.context)
-        with open(path, "r") as f:
-            p = yaml.safe_load(f)["/**"]["ros__parameters"]
-        p["vehicle_length"] = p["front_overhang"] + p["wheel_base"] + p["rear_overhang"]
-        p["vehicle_width"] = p["wheel_tread"] + p["left_overhang"] + p["right_overhang"]
-        p["min_longitudinal_offset"] = -p["rear_overhang"]
-        p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"]
-        p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"])
-        p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"]
+        # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support
+        # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py
+        gp = self.context.launch_configurations.get("ros_params", {})
+        if not gp:
+            gp = dict(self.context.launch_configurations.get("global_params", {}))
+        p = {}
+        p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"]
+        p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"]
+        p["min_longitudinal_offset"] = -gp["rear_overhang"]
+        p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"]
+        p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"])
+        p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"]
+        p["min_height_offset"] = 0.0
+        p["max_height_offset"] = gp["vehicle_height"]
         return p
 
     def get_vehicle_mirror_info(self):
@@ -263,7 +268,7 @@ def create_single_frame_obstacle_segmentation_components(self, input_topic, outp
             components.append(
                 self.get_additional_lidars_concatenated_component(
                     input_topics=[common_pipeline_output]
-                    + list(map(lambda x: f"{x}/pointcloud"), additional_lidars),
+                    + [f"{x}/pointcloud" for x in additional_lidars],
                     output_topic=relay_topic if use_ransac else output_topic,
                 )
             )
@@ -509,7 +514,6 @@ def add_launch_arg(name: str, default_value=None):
         launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
 
     add_launch_arg("base_frame", "base_link")
-    add_launch_arg("vehicle_param_file")
     add_launch_arg("use_multithread", "False")
     add_launch_arg("use_intra_process", "True")
     add_launch_arg("use_pointcloud_container", "False")

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -5,7 +5,6 @@
 
   <!-- common parameters -->
   <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the detection module"/>
-  <arg name="vehicle_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml" description="path to the file of vehicle info yaml"/>
   <arg name="mode" default="camera_lidar_fusion" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`"/>
   <arg name="image_raw0" default="/sensing/camera/camera0/image_rect_color" description="image raw topic name"/>
@@ -48,7 +47,6 @@
       <include file="$(find-pkg-share tier4_perception_launch)/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py">
         <arg name="tier4_perception_launch_param_path" value="$(var tier4_perception_launch_param_path)"/>
         <arg name="base_frame" value="base_link"/>
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
         <arg name="use_intra_process" value="true"/>
         <arg name="use_multithread" value="true"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>

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
@@ -1,8 +1,14 @@
 <?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"/>
+  <arg name="use_crosswalk_traffic_light_estimator" default="true" description="output pedestrian's traffic light signals"/>
 
   <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,11 +28,18 @@
   <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="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>
   </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
@@ -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")
 
@@ -67,6 +67,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("input_h", "224")
     add_launch_arg("input_w", "224")
 
+    add_launch_arg("use_crosswalk_traffic_light_estimator", "True")
     add_launch_arg("use_intra_process", "False")
     add_launch_arg("use_multithread", "False")
 
@@ -82,22 +83,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",
@@ -117,7 +102,7 @@ def create_parameter_dict(*args):
                 remappings=[
                     ("~/input/image", LaunchConfiguration("input/image")),
                     ("~/input/rois", "rois"),
-                    ("~/output/traffic_signals", "traffic_signals"),
+                    ("~/output/traffic_signals", "classified/traffic_signals"),
                 ],
                 extra_arguments=[
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -146,6 +131,69 @@ def create_parameter_dict(*args):
         output="both",
     )
 
+    estimator_loader = LoadComposableNodes(
+        composable_node_descriptions=[
+            ComposableNode(
+                package="crosswalk_traffic_light_estimator",
+                plugin="traffic_light::CrosswalkTrafficLightEstimatorNode",
+                name="crosswalk_traffic_light_estimator",
+                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"),
+                ],
+                extra_arguments=[{"use_intra_process_comms": False}],
+            ),
+        ],
+        target_container=container,
+        condition=IfCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")),
+    )
+
+    relay_loader = LoadComposableNodes(
+        composable_node_descriptions=[
+            ComposableNode(
+                package="topic_tools",
+                plugin="topic_tools::RelayNode",
+                name="classified_signals_relay",
+                namespace="",
+                parameters=[
+                    {"input_topic": "classified/traffic_signals"},
+                    {"output_topic": "traffic_signals"},
+                    {"type": "autoware_auto_perception_msgs/msg/TrafficSignalArray"},
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            )
+        ],
+        target_container=container,
+        condition=UnlessCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")),
+    )
+
+    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 +205,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 +223,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 +244,9 @@ def create_parameter_dict(*args):
             set_container_executable,
             set_container_mt_executable,
             container,
-            loader,
+            decompressor_loader,
+            fine_detector_loader,
+            estimator_loader,
+            relay_loader,
         ]
     )
diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml
@@ -13,6 +13,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <exec_depend>compare_map_segmentation</exec_depend>
+  <exec_depend>crosswalk_traffic_light_estimator</exec_depend>
   <exec_depend>detected_object_feature_remover</exec_depend>
   <exec_depend>detected_object_validation</exec_depend>
   <exec_depend>detection_by_tracker</exec_depend>
@@ -28,6 +29,7 @@
   <exec_depend>occupancy_grid_map_outlier_filter</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pointcloud_to_laserscan</exec_depend>
+  <exec_depend>probabilistic_occupancy_grid_map</exec_depend>
   <exec_depend>shape_estimation</exec_depend>
   <exec_depend>tensorrt_yolo</exec_depend>
   <exec_depend>traffic_light_classifier</exec_depend>

diff --git a/launch/tier4_sensing_launch/launch/sensing.launch.xml b/launch/tier4_sensing_launch/launch/sensing.launch.xml
@@ -1,7 +1,6 @@
 <launch>
   <arg name="launch_driver" default="true" description="do launch driver"/>
   <arg name="sensor_model" default="sample_sensor_kit" description="sensor model name"/>
-  <arg name="vehicle_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml" description="path to the file of vehicle info yaml"/>
   <arg name="vehicle_mirror_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_mirror.param.yaml" description="path to the file of vehicle mirror position yaml"/>
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -14,7 +13,6 @@
     <!-- Driver -->
     <include file="$(var sensor_launch_pkg)/launch/sensing.launch.xml">
       <arg name="launch_driver" value="$(var launch_driver)"/>
-      <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
       <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>