feat: implemented the launcher-side changes for the cuda preprocessing and transport layer

knzo25 · knzo25 · commit a17071bb791b · 2024-11-26T14:30:44.000+09:00
Signed-off-by: Kenzo Lobos-Tsunekawa &lt;kenzo.lobos@tier4.jp&gt;
diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Tier IV, Inc. All rights reserved.
+# Copyright 2024 Tier IV, Inc. All rights reserved.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -23,6 +23,7 @@
 from launch.conditions import UnlessCondition
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
 from launch_ros.parameter_descriptions import ParameterFile
 import yaml
@@ -101,13 +102,14 @@ def create_parameter_dict(*args):
 
     nodes = []
 
-    nodes.append(
-        ComposableNode(
-            package="glog_component",
-            plugin="GlogComponent",
-            name="glog_component",
+    if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context):
+        nodes.append(
+            ComposableNode(
+                package="glog_component",
+                plugin="GlogComponent",
+                name="glog_component",
+            )
         )
-    )
 
     nodes.append(
         ComposableNode(
@@ -149,87 +151,155 @@ def create_parameter_dict(*args):
         )
     )
 
-    cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
-    cropbox_parameters["negative"] = True
-
-    vehicle_info = get_vehicle_info(context)
-    cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"]
-    cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"]
-    cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"]
-    cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"]
-    cropbox_parameters["min_z"] = vehicle_info["min_height_offset"]
-    cropbox_parameters["max_z"] = vehicle_info["max_height_offset"]
+    if IfCondition(LaunchConfiguration("use_cuda_preprocessor")).evaluate(context):
+        nodes.append(
+            ComposableNode(
+                package="autoware_cuda_pointcloud_preprocessor",
+                plugin="autoware::cuda_organized_pointcloud_adapter::CudaOrganizedPointcloudAdapterNode",
+                name="cuda_organized_pointcloud_adapter_node",
+                remappings=[
+                    ("~/input/pointcloud", "pointcloud_raw_ex"),
+                    ("~/output/pointcloud", "cuda_points"),
+                    ("~/output/pointcloud/cuda", "cuda_points/cuda"),
+                ],
+                # The whole node can not set use_intra_process due to type negotiation internal topics
+                # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+            )
+        )
 
-    nodes.append(
-        ComposableNode(
-            package="autoware_pointcloud_preprocessor",
-            plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
-            name="crop_box_filter_self",
-            remappings=[
-                ("input", "pointcloud_raw_ex"),
-                ("output", "self_cropped/pointcloud_ex"),
-            ],
-            parameters=[cropbox_parameters],
-            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        preprocessor_parameters = {}
+
+        vehicle_info = get_vehicle_info(context)
+        preprocessor_parameters["self_crop.min_x"] = vehicle_info["min_longitudinal_offset"]
+        preprocessor_parameters["self_crop.max_x"] = vehicle_info["max_longitudinal_offset"]
+        preprocessor_parameters["self_crop.min_y"] = vehicle_info["min_lateral_offset"]
+        preprocessor_parameters["self_crop.max_y"] = vehicle_info["max_lateral_offset"]
+        preprocessor_parameters["self_crop.min_z"] = vehicle_info["min_height_offset"]
+        preprocessor_parameters["self_crop.max_z"] = vehicle_info["max_height_offset"]
+
+        mirror_info = get_vehicle_mirror_info(context)
+        preprocessor_parameters["mirror_crop.min_x"] = mirror_info["min_longitudinal_offset"]
+        preprocessor_parameters["mirror_crop.max_x"] = mirror_info["max_longitudinal_offset"]
+        preprocessor_parameters["mirror_crop.min_y"] = mirror_info["min_lateral_offset"]
+        preprocessor_parameters["mirror_crop.max_y"] = mirror_info["max_lateral_offset"]
+        preprocessor_parameters["mirror_crop.min_z"] = mirror_info["min_height_offset"]
+        preprocessor_parameters["mirror_crop.max_z"] = mirror_info["max_height_offset"]
+
+        nodes.append(
+            ComposableNode(
+                package="autoware_cuda_pointcloud_preprocessor",
+                plugin="autoware::cuda_pointcloud_preprocessor::CudaPointcloudPreprocessorNode",
+                name="cuda_pointcloud_preprocessor_node",
+                parameters=[
+                    preprocessor_parameters,
+                    distortion_corrector_node_param,
+                    ring_outlier_filter_node_param,
+                ],
+                remappings=[
+                    ("~/input/pointcloud", "cuda_points"),
+                    ("~/input/pointcloud/cuda", "cuda_points/cuda"),
+                    ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+                    ("~/input/imu", "/sensing/imu/imu_data"),
+                    ("~/output/pointcloud", "pointcloud_before_sync"),
+                    ("~/output/pointcloud/cuda", "pointcloud_before_sync/cuda"),
+                ],
+                # The whole node can not set use_intra_process due to type negotiation internal topics
+                # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+            )
         )
-    )
 
-    mirror_info = get_vehicle_mirror_info(context)
-    cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"]
-    cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"]
-    cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"]
-    cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"]
-    cropbox_parameters["min_z"] = mirror_info["min_height_offset"]
-    cropbox_parameters["max_z"] = mirror_info["max_height_offset"]
+    else:
+        cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
+        cropbox_parameters["negative"] = True
+
+        vehicle_info = get_vehicle_info(context)
+        cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"]
+        cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"]
+        cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"]
+        cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"]
+        cropbox_parameters["min_z"] = vehicle_info["min_height_offset"]
+        cropbox_parameters["max_z"] = vehicle_info["max_height_offset"]
+
+        nodes.append(
+            ComposableNode(
+                package="autoware_pointcloud_preprocessor",
+                plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
+                name="crop_box_filter_self",
+                remappings=[
+                    ("input", "pointcloud_raw_ex"),
+                    ("output", "self_cropped/pointcloud_ex"),
+                ],
+                parameters=[cropbox_parameters],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            )
+        )
 
-    nodes.append(
-        ComposableNode(
-            package="autoware_pointcloud_preprocessor",
-            plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
-            name="crop_box_filter_mirror",
-            remappings=[
-                ("input", "self_cropped/pointcloud_ex"),
-                ("output", "mirror_cropped/pointcloud_ex"),
-            ],
-            parameters=[cropbox_parameters],
-            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        mirror_info = get_vehicle_mirror_info(context)
+        cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"]
+        cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"]
+        cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"]
+        cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"]
+        cropbox_parameters["min_z"] = mirror_info["min_height_offset"]
+        cropbox_parameters["max_z"] = mirror_info["max_height_offset"]
+
+        nodes.append(
+            ComposableNode(
+                package="autoware_pointcloud_preprocessor",
+                plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
+                name="crop_box_filter_mirror",
+                remappings=[
+                    ("input", "self_cropped/pointcloud_ex"),
+                    ("output", "mirror_cropped/pointcloud_ex"),
+                ],
+                parameters=[cropbox_parameters],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            )
         )
-    )
 
-    nodes.append(
-        ComposableNode(
-            package="autoware_pointcloud_preprocessor",
-            plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
-            name="distortion_corrector_node",
-            remappings=[
-                ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
-                ("~/input/imu", "/sensing/imu/imu_data"),
-                ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"),
-                ("~/output/pointcloud", "rectified/pointcloud_ex"),
-            ],
-            parameters=[distortion_corrector_node_param],
-            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        nodes.append(
+            ComposableNode(
+                package="autoware_pointcloud_preprocessor",
+                plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
+                name="distortion_corrector_node",
+                remappings=[
+                    ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+                    ("~/input/imu", "/sensing/imu/imu_data"),
+                    ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"),
+                    ("~/output/pointcloud", "rectified/pointcloud_ex"),
+                ],
+                parameters=[distortion_corrector_node_param],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            )
         )
-    )
 
-    # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
-    if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
-        ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
-    else:
-        ring_outlier_output_frame = {"output_frame": ""}  # keep the output frame as the input frame
-    nodes.append(
-        ComposableNode(
-            package="autoware_pointcloud_preprocessor",
-            plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent",
-            name="ring_outlier_filter",
-            remappings=[
-                ("input", "rectified/pointcloud_ex"),
-                ("output", "pointcloud_before_sync"),
-            ],
-            parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
-            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+        if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
+            ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
+        else:
+            ring_outlier_output_frame = {
+                "output_frame": ""
+            }  # keep the output frame as the input frame
+        nodes.append(
+            ComposableNode(
+                package="autoware_pointcloud_preprocessor",
+                plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent",
+                name="ring_outlier_filter",
+                remappings=[
+                    ("input", "rectified/pointcloud_ex"),
+                    ("output", "pointcloud_before_sync"),
+                ],
+                parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            )
         )
-    )
 
     # set container to run all required components in the same process
     container = ComposableNodeContainer(
@@ -238,10 +308,17 @@ def create_parameter_dict(*args):
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=nodes,
+        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
         output="both",
     )
 
-    return [container]
+    load_composable_nodes = LoadComposableNodes(
+        composable_node_descriptions=nodes,
+        target_container=LaunchConfiguration("pointcloud_container_name"),
+        condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
+    )
+
+    return [container, load_composable_nodes]
 
 
 def generate_launch_description():
@@ -278,6 +355,9 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("use_multithread", "False", "use multithread")
     add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication")
     add_launch_arg("lidar_container_name", "nebula_node_container")
+    add_launch_arg("pointcloud_container_name", "pointcloud_container")
+    add_launch_arg("use_pointcloud_container", "False")
+    add_launch_arg("use_cuda_preprocessor", "False")
     add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
     add_launch_arg(
         "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
diff --git a/common_sensor_launch/launch/robosense_Bpearl.launch.xml b/common_sensor_launch/launch/robosense_Bpearl.launch.xml
@@ -15,6 +15,9 @@
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="container_name" default="robosense_node_container"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="use_cuda_preprocessor" default="false"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -33,5 +36,8 @@
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="false"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+    <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+    <arg name="use_cuda_preprocessor" value="$(var use_cuda_preprocessor)"/>
   </include>
 </launch>
diff --git a/common_sensor_launch/launch/robosense_Helios.launch.xml b/common_sensor_launch/launch/robosense_Helios.launch.xml
@@ -15,6 +15,9 @@
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="container_name" default="robosense_node_container"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="use_cuda_preprocessor" default="false"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -33,5 +36,8 @@
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="false"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+    <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+    <arg name="use_cuda_preprocessor" value="$(var use_cuda_preprocessor)"/>
   </include>
 </launch>
diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
@@ -14,6 +14,9 @@
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="container_name" default="velodyne_node_container"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="use_cuda_preprocessor" default="false"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -32,6 +35,9 @@
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="false"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+    <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+    <arg name="use_cuda_preprocessor" value="$(var use_cuda_preprocessor)"/>
   </include>
 
   <!-- Velodyne Monitor -->
diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
@@ -14,6 +14,9 @@
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="container_name" default="velodyne_node_container"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="use_cuda_preprocessor" default="false"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -32,6 +35,9 @@
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="true"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+    <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+    <arg name="use_cuda_preprocessor" value="$(var use_cuda_preprocessor)"/>
   </include>
 
   <!-- Velodyne Monitor -->
diff --git a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml
@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    use_cuda: true
     debug_mode: false
     has_static_tf_only: false
     rosbag_replay: false
diff --git a/sample_sensor_kit_launch/launch/lidar.launch.xml b/sample_sensor_kit_launch/launch/lidar.launch.xml
diff --git a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py
