From 276f58a78bd58d2b7036e01b168b9f17ac051818 Mon Sep 17 00:00:00 2001
From: Max SCHMELLER <max.schmeller@tier4.jp>
Date: Fri, 5 Jul 2024 17:57:10 +0900
Subject: [PATCH] refactor!: nebula single node support

---
 .../launch/nebula_node_container.launch.py    | 143 ++++++------------
 1 file changed, 47 insertions(+), 96 deletions(-)

diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py
index 4d7a44e7..8b3c8df1 100644
--- a/common_sensor_launch/launch/nebula_node_container.launch.py
+++ b/common_sensor_launch/launch/nebula_node_container.launch.py
@@ -12,7 +12,9 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from ctypes import ArgumentError
 import os
+from pathlib import Path
 
 from ament_index_python.packages import get_package_share_directory
 import launch
@@ -26,15 +28,16 @@
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.parameter_descriptions import ParameterFile
 import yaml
 
 
 def get_lidar_make(sensor_name):
     if sensor_name[:6].lower() == "pandar":
-        return "Hesai", ".csv"
-    elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]:
-        return "Velodyne", ".yaml"
-    return "unrecognized_sensor_model"
+        return "Hesai"
+    elif sensor_name[:3].lower() in ["vlp", "vls"]:
+        return "Velodyne"
+    raise ArgumentError("Unrecognized sensor model")
 
 
 def get_vehicle_info(context):
@@ -68,19 +71,25 @@ def create_parameter_dict(*args):
 
     # Model and make
     sensor_model = LaunchConfiguration("sensor_model").perform(context)
-    sensor_make, sensor_extension = get_lidar_make(sensor_model)
-    nebula_decoders_share_dir = get_package_share_directory("nebula_decoders")
-
-    # Calibration file
-    sensor_calib_fp = os.path.join(
-        nebula_decoders_share_dir,
-        "calibration",
-        sensor_make.lower(),
-        sensor_model + sensor_extension,
+    sensor_make = get_lidar_make(sensor_model)
+
+    # Configuration file containing sensor model's default parameters
+    
+    sensor_config = LaunchConfiguration("config_file").perform(context)
+    if (sensor_config == ""):
+        sensor_config = (
+            Path(get_package_share_directory("nebula_ros"))
+            / "config"
+            / "lidar"
+            / sensor_make.lower()
+            / (sensor_model + ".param.yaml")
+        )
+    else:
+        sensor_config = Path(sensor_config)
+
+    assert sensor_config.exists(), "Sensor configuration not found: {}".format(
+        sensor_config.absolute().as_posix()
     )
-    assert os.path.exists(
-        sensor_calib_fp
-    ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp)
 
     nodes = []
 
@@ -95,26 +104,35 @@ def create_parameter_dict(*args):
     nodes.append(
         ComposableNode(
             package="nebula_ros",
-            plugin=sensor_make + "DriverRosWrapper",
-            name=sensor_make.lower() + "_driver_ros_wrapper_node",
+            plugin=sensor_make + "RosWrapper",
+            name=sensor_make.lower() + "_ros_wrapper_node",
             parameters=[
+                ParameterFile(sensor_config, allow_substs=True),
                 {
-                    "calibration_file": sensor_calib_fp,
                     "sensor_model": sensor_model,
+                    "launch_hw": LaunchConfiguration("launch_driver"),
                     **create_parameter_dict(
                         "host_ip",
                         "sensor_ip",
                         "data_port",
-                        "return_mode",
+                        "gnss_port",
+                        "packet_mtu_size",
+                        "setup_sensor",
+                        "frame_id",
+                        "diag_span",
                         "min_range",
                         "max_range",
-                        "frame_id",
-                        "scan_phase",
                         "cloud_min_angle",
                         "cloud_max_angle",
-                        "dual_return_distance_threshold",
-                        "setup_sensor",
+                        "scan_phase",
+                        "rotation_speed",
+                        "return_mode",
+                        "ptp_profile",
+                        "ptp_domain",
+                        "ptp_transport_type",
+                        "ptp_switch_type",
                         "retry_hw",
+                        "dual_return_distance_threshold",
                     ),
                 },
             ],
@@ -124,37 +142,9 @@ def create_parameter_dict(*args):
                 ("pandar_points", "pointcloud_raw_ex"),
                 ("velodyne_points", "pointcloud_raw_ex"),
             ],
-            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-        )
-    )
-
-    nodes.append(
-        ComposableNode(
-            package="nebula_ros",
-            plugin=sensor_make + "HwMonitorRosWrapper",
-            name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node",
-            parameters=[
-                {
-                    "sensor_model": sensor_model,
-                    **create_parameter_dict(
-                        "return_mode",
-                        "frame_id",
-                        "scan_phase",
-                        "sensor_ip",
-                        "host_ip",
-                        "data_port",
-                        "gnss_port",
-                        "packet_mtu_size",
-                        "rotation_speed",
-                        "cloud_min_angle",
-                        "cloud_max_angle",
-                        "diag_span",
-                        "dual_return_distance_threshold",
-                        "delay_monitor_ms",
-                    ),
-                },
+            extra_arguments=[
+                {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
             ],
-            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
     )
 
@@ -252,39 +242,6 @@ def create_parameter_dict(*args):
         output="both",
     )
 
-    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="pointcloud_preprocessor",
         plugin="pointcloud_preprocessor::BlockageDiagComponent",
@@ -310,18 +267,13 @@ def create_parameter_dict(*args):
         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]
+    return [container, blockage_diag_loader]
 
 
 def generate_launch_description():
@@ -335,8 +287,8 @@ def add_launch_arg(name: str, default_value=None, description=None):
 
     add_launch_arg("sensor_model", description="sensor model name")
     add_launch_arg("config_file", "", description="sensor configuration file")
-    add_launch_arg("launch_driver", "True", "do launch driver")
-    add_launch_arg("setup_sensor", "True", "configure sensor")
+    add_launch_arg("launch_driver", "true", "whether to connect to a sensor or to listen to packet replays instead")
+    add_launch_arg("setup_sensor", "true", "configure sensor")
     add_launch_arg("retry_hw", "false", "retry hw")
     add_launch_arg("sensor_ip", "192.168.1.201", "device ip address")
     add_launch_arg("host_ip", "255.255.255.255", "host ip address")
@@ -366,7 +318,6 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("ptp_domain", "0")
     add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
     add_launch_arg("diag_span", "1000", "")
-    add_launch_arg("delay_monitor_ms", "2000", "")
     add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
 
     add_launch_arg("enable_blockage_diag", "true")