fix: enable blocakge diag for xx1

Signed-off-by: badai-nguyen <[email protected]>

aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml

@@ -18,6 +18,15 @@
           type: diagnostic_aggregator/AnalyzerGroup
           path: lidar
           analyzers:
+            performance_monitoring:
+              type: diagnostic_aggregator/AnalyzerGroup
+              path: performance_monitoring
+              analyzers:
+                blockage:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: blockage
+                  contains: [": blockage_validation"]
+                  timeout: 1.0
             velodyne:
               type: diagnostic_aggregator/AnalyzerGroup
               path: velodyne

aip_xx1_launch/launch/lidar.launch.xml

@@ -6,6 +6,7 @@
   <arg name="vehicle_mirror_param_file" />
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="launch_blockage_diag" default="true"/>
 
   <group>
     <push-ros-namespace namespace="lidar"/>
@@ -22,6 +23,11 @@
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
+        <arg name="angle_range" value="[0.0, 360.0]"/>
+        <arg name="vertical_bins" value="128"/>
+        <arg name="horizontal_ring_id" value="64"/>
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="launch_blockage_diag" value="$(var launch_blockage_diag)"/>
       </include>
     </group>
 
@@ -39,6 +45,11 @@
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
+        <arg name="angle_range" value="[90.0, 270.0]"/>
+        <arg name="vertical_bins" value="16"/>
+        <arg name="horizontal_ring_id" value="16"/>
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="launch_blockage_diag" value="$(var launch_blockage_diag)"/>
       </include>
     </group>
 
@@ -56,6 +67,11 @@
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
+        <arg name="angle_range" value="[90.0, 270.0]"/>
+        <arg name="vertical_bins" value="16"/>
+        <arg name="horizontal_ring_id" value="16"/>
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="launch_blockage_diag" value="$(var launch_blockage_diag)"/>
       </include>
     </group>
 
@@ -73,6 +89,11 @@
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
+        <arg name="angle_range" value="[90.0, 270.0]"/>
+        <arg name="vertical_bins" value="16"/>
+        <arg name="horizontal_ring_id" value="16"/>
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="launch_blockage_diag" value="$(var launch_blockage_diag)"/>
       </include>
     </group>
 

common_sensor_launch/config/blockage_diagnostics_param_file.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+    blockage_ratio_threshold: 0.1
+    blockage_count_threshold: 50
+    blockage_buffering_frames: 2
+    blockage_buffering_interval: 1
+    dust_ratio_threshold: 0.2
+    dust_count_threshold: 10
+    dust_kernel_size: 2
+    dust_buffering_frames: 10
+    dust_buffering_interval: 1
+    distance_coefficient: 327.67
+    horizontal_resolution: 0.4

common_sensor_launch/launch/velodyne_node_container.launch.py

@@ -22,6 +22,7 @@
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
 import yaml
 
 
@@ -43,14 +44,12 @@ 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 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:
@@ -114,7 +113,7 @@ def create_parameter_dict(*args):
         )
     )
 
-    mirror_info = get_vehicle_mirror_info(context)
+    mirror_info = load_composable_node_param("vehicle_mirror_param_file")
     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"]
@@ -205,6 +204,25 @@ def create_parameter_dict(*args):
         ],
     )
 
+    blockage_diag_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin="pointcloud_preprocessor::BlockageDiagComponent",
+        name="blockage_diag",
+        remappings=[
+            ("input", "pointcloud_raw_ex"),
+            ("output", "blockage_diag/pointcloud"),
+        ],
+        parameters=[
+            {
+                "angle_range":LaunchConfiguration("angle_range"),
+                "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
+                "vertical_bins": LaunchConfiguration("vertical_bins"),
+                "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
+            }
+        ]+ [load_composable_node_param("blockage_diagnostics_param_file")],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
     target_container = (
         container
         if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
@@ -216,8 +234,13 @@ def create_parameter_dict(*args):
         target_container=target_container,
         condition=IfCondition(LaunchConfiguration("launch_driver")),
     )
+    blockage_diag_loader = LoadComposableNodes(
+        composable_node_descriptions=[blockage_diag_component],
+        target_container=target_container,
+        condition=IfCondition(LaunchConfiguration("launch_blockage_diag")),
+    )
 
-    return [container, component_loader, driver_component_loader]
+    return [container, component_loader, driver_component_loader,blockage_diag_loader]
 
 
 def generate_launch_description():
@@ -262,6 +285,15 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("use_pointcloud_container", "false")
     add_launch_arg("container_name", "velodyne_node_container")
 
+    add_launch_arg("angle_range","[0.0, 360]")
+    add_launch_arg("horizontal_ring_id","64")
+    add_launch_arg("vertical_bins","128")
+    add_launch_arg("is_channel_order_top2down","true")
+    add_launch_arg(
+        "blockage_diagnostics_param_file",
+        [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics_param_file.yaml"],
+    )
+
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
         "component_container",