From 714db941d63936f35667ffb0cfac2125c4a6f19f Mon Sep 17 00:00:00 2001
From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>
Date: Tue, 19 Mar 2024 18:26:25 +0900
Subject: [PATCH 1/2] chore(blockage_diag): update param for aip_x2_launch
 (#223)

* chore: update blockage param for aip_x2_launch

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: update param config

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

---------

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../blockage_diagnostics_param_file.yaml      |  1 +
 aip_x2_launch/launch/lidar.launch.xml         | 25 +++++++++++++++++++
 .../launch/pandar_node_container.launch.py    | 10 +++++++-
 3 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
index b8b62b50..c6ce8e11 100644
--- a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
+++ b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
@@ -9,3 +9,4 @@
     dust_kernel_size: 2
     dust_buffering_frames: 10
     dust_buffering_interval: 1
+    blockage_kernel: 10
diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml
index 523add57..ba75c4c8 100644
--- a/aip_x2_launch/launch/lidar.launch.xml
+++ b/aip_x2_launch/launch/lidar.launch.xml
@@ -7,6 +7,7 @@
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
   <arg name="dual_return_filter_param_file" default="$(find-pkg-share aip_x2_launch)/config/dual_return_filter.param.yaml"/>
+  <arg name="enable_blockage_diag" default="true"/>
 
   <group>
     <push-ros-namespace namespace="lidar" />
@@ -27,6 +28,9 @@
         <arg name="return_mode" value="Dual" />
         <arg name="min_azimuth_deg" value="135.0"/>
         <arg name="max_azimuth_deg" value="225.0"/>
+        <arg name="is_channel_order_top2down" value="true"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
@@ -48,6 +52,9 @@
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="40" />
         <arg name="return_mode" value="First" />
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.6"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
@@ -71,6 +78,9 @@
         <arg name="horizontal_ring_id" value="12" />
         <arg name="min_azimuth_deg" value="225.0"/>
         <arg name="max_azimuth_deg" value="315.0"/>
+        <arg name="is_channel_order_top2down" value="true"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="Strongest" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -93,6 +103,9 @@
         <arg name="blockage_range" value="[120.0, 240.0]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="8" />
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.6"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -117,6 +130,9 @@
         <arg name="horizontal_ring_id" value="12" />
         <arg name="min_azimuth_deg" value="45.0"/>
         <arg name="max_azimuth_deg" value="135.0"/>
+        <arg name="is_channel_order_top2down" value="true"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="Strongest" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -139,6 +155,9 @@
         <arg name="blockage_range" value="[150.0, 240.0]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="8" />
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.6"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -163,6 +182,9 @@
         <arg name="horizontal_ring_id" value="12" />
         <arg name="min_azimuth_deg" value="135.0"/>
         <arg name="max_azimuth_deg" value="225.0"/>
+        <arg name="is_channel_order_top2down" value="true"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="Strongest" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -185,6 +207,9 @@
         <arg name="blockage_range" value="[90.0, 270.0]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="40" />
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.6"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py
index 43db6570..8d8b13cf 100644
--- a/aip_x2_launch/launch/pandar_node_container.launch.py
+++ b/aip_x2_launch/launch/pandar_node_container.launch.py
@@ -43,6 +43,10 @@ def get_pandar_monitor_info():
     return p
 
 
+def str2vector(string):
+    return [float(x) for x in string.strip("[]").split(",")]
+
+
 def get_vehicle_info(context):
     # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support
     # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py
@@ -232,6 +236,7 @@ def create_parameter_dict(*args):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range")))
     blockage_diag_component = ComposableNode(
         package="pointcloud_preprocessor",
         plugin="pointcloud_preprocessor::BlockageDiagComponent",
@@ -245,7 +250,9 @@ def create_parameter_dict(*args):
                 "angle_range": LaunchConfiguration("blockage_range"),
                 "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
                 "vertical_bins": LaunchConfiguration("vertical_bins"),
-                "model": LaunchConfiguration("model"),
+                "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
+                "max_distance_range": distance_range[1],
+                "horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
             }
         ]
         + [load_composable_node_param("blockage_diagnostics_param_file")],
@@ -323,6 +330,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("input_frame", LaunchConfiguration("base_frame"))
     add_launch_arg("output_frame", LaunchConfiguration("base_frame"))
     add_launch_arg("dual_return_filter_param_file")
+    add_launch_arg("horizontal_resolution", "0.4")
     add_launch_arg(
         "blockage_diagnostics_param_file",
         [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"],

From ae0ab3d4919c7b6fb8505930f1d51976eef2f43a Mon Sep 17 00:00:00 2001
From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>
Date: Fri, 29 Mar 2024 15:21:42 +0900
Subject: [PATCH 2/2] chore(blockage_diag): add option param for x2 (#224)

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 aip_x2_launch/config/blockage_diagnostics_param_file.yaml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
index c6ce8e11..e43b2039 100644
--- a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
+++ b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml
@@ -4,6 +4,8 @@
     blockage_count_threshold: 50
     blockage_buffering_frames: 2
     blockage_buffering_interval: 1
+    enable_dust_diag: false
+    publish_debug_image: false
     dust_ratio_threshold: 0.2
     dust_count_threshold: 10
     dust_kernel_size: 2