fix: add blockage for xx1 (tier4#191)

* fix: enable blocakge diag for xx1 Signed-off-by: badai-nguyen <[email protected]> * fix: xx1 launch Signed-off-by: badai-nguyen <[email protected]> * fix: x2 launch Signed-off-by: badai-nguyen <[email protected]> * ci(pre-commit): autofix * fix: remove duplicated angle_range param Signed-off-by: badai-nguyen <[email protected]> * fix: remove x2 redundant param Signed-off-by: badai-nguyen <[email protected]> * chore: update config file Signed-off-by: badai-nguyen <[email protected]> * chore: disable xx1 blockage as default Signed-off-by: badai-nguyen <[email protected]> * fix: velodyne launch Signed-off-by: badai-nguyen <[email protected]> * Revert "fix: remove x2 redundant param" This reverts commit fa93588. * Revert "fix: x2 launch" This reverts commit 4748c45. --------- Signed-off-by: badai-nguyen <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
badai-nguyen · Mar 21, 2024 · 2f88702 · 2f88702
1 parent b900f4b
commit 2f88702
Show file tree

Hide file tree

Showing 8 changed files with 126 additions and 9 deletions.
diff --git a/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/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

diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml
@@ -6,6 +6,7 @@
   <arg name ="vehicle_id" default="$(env VEHICLE_ID default)"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="enable_blockage_diag" default="false"/>
 
   <group>
     <push-ros-namespace namespace="lidar"/>
@@ -22,6 +23,11 @@
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
         <arg name="container_name" value="pointcloud_container"/>
+        <arg name="vertical_bins" value="128"/>
+        <arg name="horizontal_ring_id" value="64"/>
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
       </include>
     </group>
 
@@ -39,6 +45,11 @@
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
         <arg name="container_name" value="pointcloud_container"/>
+        <arg name="vertical_bins" value="16"/>
+        <arg name="horizontal_ring_id" value="0"/>
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
       </include>
     </group>
 
@@ -56,6 +67,11 @@
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
         <arg name="container_name" value="pointcloud_container"/>
+        <arg name="vertical_bins" value="16"/>
+        <arg name="horizontal_ring_id" value="0"/>
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
       </include>
     </group>
 
@@ -73,6 +89,11 @@
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
         <arg name="container_name" value="pointcloud_container"/>
+        <arg name="vertical_bins" value="16"/>
+        <arg name="horizontal_ring_id" value="0"/>
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
       </include>
     </group>
 

diff --git a/common_sensor_launch/CMakeLists.txt b/common_sensor_launch/CMakeLists.txt
@@ -12,4 +12,5 @@ endif()
 ament_auto_package(INSTALL_TO_SHARE
   config
   launch
+  config
 )
diff --git a/common_sensor_launch/config/blockage_diagnostics_param_file.yaml b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml
@@ -0,0 +1,12 @@
+/**:
+  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
+    blockage_kernel: 10
diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py
@@ -25,6 +25,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
 
 
@@ -54,14 +55,11 @@ 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:
@@ -151,7 +149,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"]
@@ -239,13 +237,49 @@ 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": [
+                    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")}],
+    )
+
+    target_container = (
+        container
+        if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
+        else LaunchConfiguration("container_name")
+    )
+
     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=target_container,
+        condition=IfCondition(LaunchConfiguration("enable_blockage_diag")),
+    )
 
-    return [container, driver_component_loader]
+    return [container, driver_component_loader, blockage_diag_loader]
 
 
 def generate_launch_description():
@@ -284,6 +318,16 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication")
     add_launch_arg("container_name", "nebula_node_container")
 
+    add_launch_arg("enable_blockage_diag", "true")
+    add_launch_arg("horizontal_ring_id", "64")
+    add_launch_arg("vertical_bins", "128")
+    add_launch_arg("is_channel_order_top2down", "true")
+    add_launch_arg("horizontal_resolution", "0.4")
+    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",

diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
@@ -16,6 +16,11 @@
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="container_name" default="velodyne_node_container"/>
+  <arg name="vertical_bins" default="16"/>
+  <arg name="horizontal_ring_id" default="0"/>
+  <arg name="is_channel_order_top2down" default="false"/>
+  <arg name="horizontal_resolution" default="0.4"/>
+  <arg name="enable_blockage_diag" default="true"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -34,6 +39,11 @@
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="false"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <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)"/>
+    <arg name="horizontal_resolution" value="$(var horizontal_resolution)"/>
+    <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
   </include>
 
   <!-- Velodyne Monitor -->

diff --git a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml
@@ -16,6 +16,11 @@
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="container_name" default="velodyne_node_container"/>
+  <arg name="vertical_bins" default="32"/>
+  <arg name="horizontal_ring_id" default="0"/>
+  <arg name="is_channel_order_top2down" default="false"/>
+  <arg name="horizontal_resolution" default="0.4"/>
+  <arg name="enable_blockage_diag" default="true"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -34,6 +39,11 @@
     <arg name="use_intra_process" value="false"/>
     <arg name="use_multithread" value="false"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <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)"/>
+    <arg name="horizontal_resolution" value="$(var horizontal_resolution)"/>
+    <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
   </include>
 
   <!-- Velodyne Monitor -->

diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
@@ -16,6 +16,11 @@
   <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="container_name" default="velodyne_node_container"/>
+  <arg name="vertical_bins" default="128"/>
+  <arg name="horizontal_ring_id" default="64"/>
+  <arg name="is_channel_order_top2down" default="false"/>
+  <arg name="horizontal_resolution" default="0.4"/>
+  <arg name="enable_blockage_diag" default="false"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -34,6 +39,11 @@
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="true"/>
     <arg name="container_name" value="$(var container_name)"/>
+    <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)"/>
+    <arg name="horizontal_resolution" value="$(var horizontal_resolution)"/>
+    <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
   </include>
 
   <!-- Velodyne Monitor -->
-Original file line number
+Diff line change
@@ Expand Up / @@ -12,4 +12,5 @@ endif() @@
     ament_auto_package(INSTALL_TO_SHARE
       config
       launch
+      config
     )