From 00bf681dbf0ff68018b49aafd9af77fc00b75942 Mon Sep 17 00:00:00 2001
From: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Date: Fri, 20 Dec 2024 13:25:04 +0900
Subject: [PATCH] Restore driver_component_loader and blockage_diag_loader

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
---
 .../launch/nebula_node_container.launch.py    | 89 ++++++++++++++++++-
 1 file changed, 88 insertions(+), 1 deletion(-)

diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py
index 6a544ca5..1b17f8ba 100644
--- a/common_sensor_launch/launch/nebula_node_container.launch.py
+++ b/common_sensor_launch/launch/nebula_node_container.launch.py
@@ -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
@@ -64,6 +65,10 @@ def get_vehicle_mirror_info(context):
 
 
 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:
@@ -218,6 +223,7 @@ def create_parameter_dict(*args):
     else:
         # keep the output frame as the input frame
         ring_outlier_output_frame = {"output_frame": ""}
+
     nodes.append(
         ComposableNode(
             package="autoware_pointcloud_preprocessor",
@@ -242,7 +248,88 @@ def create_parameter_dict(*args):
         output="both",
     )
 
-    return [container]
+    driver_component = ComposableNode(
+        package="nebula_ros",
+        plugin=sensor_make + "HwInterfaceRosWrapper",
+        # node is created in a global context, need to avoid name clash
+        name=sensor_make.lower() + "_hw_interface_ros_wrapper_node",
+        parameters=[
+            {
+                "sensor_model": sensor_model,
+                "calibration_file": sensor_calib_fp,
+                **create_parameter_dict(
+                    "sensor_ip",
+                    "host_ip",
+                    "scan_phase",
+                    "return_mode",
+                    "frame_id",
+                    "rotation_speed",
+                    "data_port",
+                    "gnss_port",
+                    "cloud_min_angle",
+                    "cloud_max_angle",
+                    "packet_mtu_size",
+                    "dual_return_distance_threshold",
+                    "setup_sensor",
+                    "ptp_profile",
+                    "ptp_transport_type",
+                    "ptp_switch_type",
+                    "ptp_domain",
+                    "retry_hw",
+                ),
+            }
+        ],
+    )
+
+    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")}
+        ],
+    )
+
+    driver_component_loader = LoadComposableNodes(
+        composable_node_descriptions=[driver_component],
+        target_container=container,
+        condition=IfCondition(LaunchConfiguration("launch_driver")),
+    )
+    blockage_diag_loader = LoadComposableNodes(
+        composable_node_descriptions=[blockage_diag_component],
+        target_container=container,
+        condition=IfCondition(LaunchConfiguration("enable_blockage_diag")),
+    )
+
+    return [container, driver_component_loader, blockage_diag_loader]
 
 
 def generate_launch_description():