feat: acceleration and transport layer (#348)

* feat: initialize parameters for concatenate

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

* chore: add xx1 gen2 parameters

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

* chore: fix aip x1 parameter

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

* chore: fix x2 parameter

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

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

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: accepting variable number of crop boxes now

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: updated the launchers to reflect the newest changes in the nodes implementation

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: refactored names

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: refactored to simply branches

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: added an assert to guarantee that the cuda preprocessor will be used with shared containers

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: added documentation for the new arguments

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: reimplementing the aip naming change instead of resolving conflicts

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: refactored node creation

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: removed the shared_container_name. Now all containers will be called pointcloud_container. The difference being whether they are loaded in the global namespace or created in a new container within the lidar namespace

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

---------

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Co-authored-by: vividf <yihsiang.fang@tier4.jp>
Co-authored-by: Yi-Hsiang Fang (Vivid) <146902905+vividf@users.noreply.github.com>

Author: 2 people

aip_common_sensor_launch/launch/hesai_OT128.launch.xml

@@ -14,7 +14,9 @@
   <arg name="cloud_max_angle" default="360"/>
   <arg name="dual_return_distance_threshold" default="0.1"/>
   <arg name="vehicle_mirror_param_file"/>
-  <arg name="container_name" default="hesai_node_container"/>
+  <arg name="container_name" default="pointcloud_container"/>
+  <arg name="use_shared_container" default="false"/>
+  <arg name="use_cuda_preprocessor" default="false"/>
   <arg name="udp_only" default="false"/>
 
   <include file="$(find-pkg-share aip_common_sensor_launch)/launch/nebula_node_container.launch.py">
@@ -31,6 +33,8 @@
     <arg name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <arg name="use_shared_container" value="$(var use_shared_container)"/>
+    <arg name="use_cuda_preprocessor" value="$(var use_cuda_preprocessor)"/>
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="true"/>
     <arg name="ptp_profile" value="automotive"/>

aip_common_sensor_launch/launch/hesai_XT32.launch.xml

@@ -15,7 +15,9 @@
   <arg name="max_range" default="120.0"/>
   <arg name="dual_return_distance_threshold" default="0.1"/>
   <arg name="vehicle_mirror_param_file"/>
-  <arg name="container_name" default="hesai_node_container"/>
+  <arg name="container_name" default="pointcloud_container"/>
+  <arg name="use_shared_container" default="false"/>
+  <arg name="use_cuda_preprocessor" default="false"/>
   <arg name="udp_only" default="false"/>
 
   <include file="$(find-pkg-share aip_common_sensor_launch)/launch/nebula_node_container.launch.py">
@@ -33,6 +35,8 @@
     <arg name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <arg name="use_shared_container" value="$(var use_shared_container)"/>
+    <arg name="use_cuda_preprocessor" value="$(var use_cuda_preprocessor)"/>
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="true"/>
     <arg name="ptp_profile" value="automotive"/>

aip_common_sensor_launch/launch/nebula_node_container.launch.py

@@ -1,4 +1,4 @@
-# Copyright 2023 Tier IV, Inc. All rights reserved.
+# Copyright 2025 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.
@@ -57,24 +57,32 @@ def get_vehicle_info(context):
     return p
 
 
-def get_vehicle_mirror_info(context):
-    path = LaunchConfiguration("vehicle_mirror_param_file").perform(context)
-    with open(path, "r") as f:
-        p = yaml.safe_load(f)["/**"]["ros__parameters"]
-    return p
+def load_composable_node_param(context, param_path):
+    with open(LaunchConfiguration(param_path).perform(context), "r") as f:
+        return yaml.safe_load(f)["/**"]["ros__parameters"]
 
 
-def launch_setup(context, *args, **kwargs):
-    def load_composable_node_param(param_path):
-        with open(LaunchConfiguration(param_path).perform(context), "r") as f:
-            return yaml.safe_load(f)["/**"]["ros__parameters"]
+def create_parameter_dict(*args):
+    result = {}
+    for x in args:
+        result[x] = LaunchConfiguration(x)
+    return result
+
+
+def make_common_nodes(context):
+    if UnlessCondition(LaunchConfiguration("use_shared_container")).evaluate(context):
+        return [
+            ComposableNode(
+                package="autoware_glog_component",
+                plugin="autoware::glog_component::GlogComponent",
+                name="glog_component",
+            )
+        ]
+
+    return []
 
-    def create_parameter_dict(*args):
-        result = {}
-        for x in args:
-            result[x] = LaunchConfiguration(x)
-        return result
 
+def make_nebula_nodes(context):
     # Model and make
     sensor_model = LaunchConfiguration("sensor_model").perform(context)
     sensor_make, sensor_extension = get_lidar_make(sensor_model)
@@ -94,27 +102,7 @@ def create_parameter_dict(*args):
     else:  # Robosense
         sensor_calib_fp = ""
 
-    # Pointcloud preprocessor parameters
-    distortion_corrector_node_param = ParameterFile(
-        param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
-        allow_substs=True,
-    )
-    ring_outlier_filter_node_param = ParameterFile(
-        param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
-        allow_substs=True,
-    )
-
-    nodes = []
-
-    nodes.append(
-        ComposableNode(
-            package="autoware_glog_component",
-            plugin="autoware::glog_component::GlogComponent",
-            name="glog_component",
-        )
-    )
-
-    nodes.append(
+    return [
         ComposableNode(
             package="nebula_ros",
             plugin=sensor_make + "RosWrapper",
@@ -163,6 +151,86 @@ def create_parameter_dict(*args):
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
+    ]
+
+
+def make_cuda_preprocessor_nodes(context):
+    # Vehicle parameters
+    vehicle_info = get_vehicle_info(context)
+    mirror_info = load_composable_node_param(context, "vehicle_mirror_param_file")
+
+    # Pointcloud preprocessor parameters
+    distortion_corrector_node_param = ParameterFile(
+        param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
+        allow_substs=True,
+    )
+    ring_outlier_filter_node_param = ParameterFile(
+        param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
+        allow_substs=True,
+    )
+
+    preprocessor_parameters = {}
+    preprocessor_parameters["crop_box.min_x"] = [
+        vehicle_info["min_longitudinal_offset"],
+        mirror_info["min_longitudinal_offset"],
+    ]
+    preprocessor_parameters["crop_box.max_x"] = [
+        vehicle_info["max_longitudinal_offset"],
+        mirror_info["max_longitudinal_offset"],
+    ]
+    preprocessor_parameters["crop_box.min_y"] = [
+        vehicle_info["min_lateral_offset"],
+        mirror_info["min_lateral_offset"],
+    ]
+    preprocessor_parameters["crop_box.max_y"] = [
+        vehicle_info["max_lateral_offset"],
+        mirror_info["max_lateral_offset"],
+    ]
+    preprocessor_parameters["crop_box.min_z"] = [
+        vehicle_info["min_height_offset"],
+        mirror_info["min_height_offset"],
+    ]
+    preprocessor_parameters["crop_box.max_z"] = [
+        vehicle_info["max_height_offset"],
+        mirror_info["max_height_offset"],
+    ]
+
+    return [
+        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", "pointcloud_raw_ex"),
+                ("~/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")}],
+        )
+    ]
+
+
+def make_preprocessor_nodes(context):
+    # Vehicle parameters
+    vehicle_info = get_vehicle_info(context)
+    mirror_info = load_composable_node_param(context, "vehicle_mirror_param_file")
+
+    # Pointcloud preprocessor parameters
+    distortion_corrector_node_param = ParameterFile(
+        param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
+        allow_substs=True,
+    )
+    ring_outlier_filter_node_param = ParameterFile(
+        param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
+        allow_substs=True,
     )
 
     cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
@@ -176,6 +244,8 @@ def create_parameter_dict(*args):
     cropbox_parameters["min_z"] = vehicle_info["min_height_offset"]
     cropbox_parameters["max_z"] = vehicle_info["max_height_offset"]
 
+    nodes = []
+
     nodes.append(
         ComposableNode(
             package="autoware_pointcloud_preprocessor",
@@ -190,7 +260,6 @@ def create_parameter_dict(*args):
         )
     )
 
-    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"]
@@ -249,6 +318,58 @@ def create_parameter_dict(*args):
         )
     )
 
+    return nodes
+
+
+def make_blockage_diag_nodes(context):
+    return [
+        ComposableNode(
+            package="autoware_pointcloud_preprocessor",
+            plugin="autoware::pointcloud_preprocessor::BlockageDiagComponent",
+            name="blockage_diag",
+            remappings=[
+                ("input", "pointcloud_raw_ex"),
+                ("output", "blockage_diag/pointcloud"),
+            ],
+            parameters=[
+                {
+                    "angle_range": [
+                        float(context.perform_substitution(LaunchConfiguration("cloud_min_angle"))),
+                        float(context.perform_substitution(LaunchConfiguration("cloud_max_angle"))),
+                    ],
+                    "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
+                    "vertical_bins": LaunchConfiguration("vertical_bins"),
+                    "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
+                    "max_distance_range": LaunchConfiguration("max_range"),
+                    "horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
+                }
+            ]
+            + [load_composable_node_param(context, "blockage_diagnostics_param_file")],
+            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        )
+    ]
+
+
+def launch_setup(context, *args, **kwargs):
+    # Check that the cuda preprocessor is only used with a shared container
+    if IfCondition(LaunchConfiguration("use_cuda_preprocessor")).evaluate(context):
+        assert IfCondition(LaunchConfiguration("use_shared_container")).evaluate(
+            context
+        ), "The cuda preprocessor should only be used with a shared container."
+
+    nodes = []
+
+    nodes.extend(make_common_nodes(context))
+    nodes.extend(make_nebula_nodes(context))
+
+    if IfCondition(LaunchConfiguration("use_cuda_preprocessor")).evaluate(context):
+        nodes.extend(make_cuda_preprocessor_nodes(context))
+    else:
+        nodes.extend(make_preprocessor_nodes(context))
+
+    if IfCondition(LaunchConfiguration("enable_blockage_diag")).evaluate(context):
+        nodes.extend(make_blockage_diag_nodes(context))
+
     # set container to run all required components in the same process
     container = ComposableNodeContainer(
         name=LaunchConfiguration("container_name"),
@@ -257,40 +378,16 @@ def create_parameter_dict(*args):
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=nodes,
         output="both",
+        condition=UnlessCondition(LaunchConfiguration("use_shared_container")),
     )
 
-    blockage_diag_component = ComposableNode(
-        package="autoware_pointcloud_preprocessor",
-        plugin="autoware::pointcloud_preprocessor::BlockageDiagComponent",
-        name="blockage_diag",
-        remappings=[
-            ("input", "pointcloud_raw_ex"),
-            ("output", "blockage_diag/pointcloud"),
-        ],
-        parameters=[
-            {
-                "angle_range": [
-                    float(context.perform_substitution(LaunchConfiguration("cloud_min_angle"))),
-                    float(context.perform_substitution(LaunchConfiguration("cloud_max_angle"))),
-                ],
-                "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
-                "vertical_bins": LaunchConfiguration("vertical_bins"),
-                "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
-                "max_distance_range": LaunchConfiguration("max_range"),
-                "horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
-            }
-        ]
-        + [load_composable_node_param("blockage_diagnostics_param_file")],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-
-    blockage_diag_loader = LoadComposableNodes(
-        composable_node_descriptions=[blockage_diag_component],
-        target_container=container,
-        condition=IfCondition(LaunchConfiguration("enable_blockage_diag")),
+    load_composable_nodes = LoadComposableNodes(
+        composable_node_descriptions=nodes,
+        target_container=LaunchConfiguration("container_name"),
+        condition=IfCondition(LaunchConfiguration("use_shared_container")),
     )
 
-    return [container, blockage_diag_loader]
+    return [container, load_composable_nodes]
 
 
 def generate_launch_description():
@@ -336,7 +433,21 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("advanced_diagnostics", "false")
     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(
+        "lidar_container_name",
+        "nebula_node_container",
+        "Name of the new container to be created when use_shared_container is false",
+    )
+    add_launch_arg(
+        "use_shared_container",
+        "False",
+        "Whether to use a new container for this lidar or use an existing one",
+    )
+    add_launch_arg(
+        "use_cuda_preprocessor",
+        "False",
+        "Use the cuda implementation of the pointcloud preprocessor. Requires use_shared_container to be enabled",
+    )
     add_launch_arg("ptp_profile", "1588v2")
     add_launch_arg("ptp_transport_type", "L2")
     add_launch_arg("ptp_switch_type", "TSN")

aip_common_sensor_launch/launch/velodyne_VLP16.launch.xml

@@ -15,7 +15,9 @@
   <arg name="cloud_min_angle" default="0"/>
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
-  <arg name="container_name" default="velodyne_node_container"/>
+  <arg name="container_name" default="pointcloud_container"/>
+  <arg name="use_shared_container" default="false"/>
+  <arg name="use_cuda_preprocessor" default="false"/>
   <arg name="vertical_bins" default="16"/>
   <arg name="horizontal_ring_id" default="0"/>
   <arg name="is_channel_order_top2down" default="false"/>
@@ -40,6 +42,8 @@
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="true"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <arg name="use_shared_container" value="$(var use_shared_container)"/>
+    <arg name="use_cuda_preprocessor" value="$(var use_cuda_preprocessor)"/>
     <arg name="vertical_bins" value="$(var vertical_bins)"/>
     <arg name="horizontal_ring_id" value="$(var horizontal_ring_id)"/>
     <arg name="is_channel_order_top2down" value="$(var is_channel_order_top2down)"/>

aip_common_sensor_launch/launch/velodyne_VLP32C.launch.xml

@@ -15,7 +15,9 @@
   <arg name="cloud_min_angle" default="0"/>
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
-  <arg name="container_name" default="velodyne_node_container"/>
+  <arg name="container_name" default="pointcloud_container"/>
+  <arg name="use_shared_container" default="false"/>
+  <arg name="use_cuda_preprocessor" default="false"/>
   <arg name="vertical_bins" default="32"/>
   <arg name="horizontal_ring_id" default="0"/>
   <arg name="is_channel_order_top2down" default="false"/>
@@ -40,6 +42,8 @@
     <arg name="use_intra_process" value="false"/>
     <arg name="use_multithread" value="false"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <arg name="use_shared_container" value="$(var use_shared_container)"/>
+    <arg name="use_cuda_preprocessor" value="$(var use_cuda_preprocessor)"/>
     <arg name="vertical_bins" value="$(var vertical_bins)"/>
     <arg name="horizontal_ring_id" value="$(var horizontal_ring_id)"/>
     <arg name="is_channel_order_top2down" value="$(var is_channel_order_top2down)"/>