From ebd4d86145b250d46de8fc6eecab34146e11ef29 Mon Sep 17 00:00:00 2001
From: kotaro-hihara <kotaro.hihara@tier4.jp>
Date: Wed, 9 Oct 2024 08:47:15 +0900
Subject: [PATCH] feat: filter points by ring_section_filter in nebula (#329)

* set point_filters test

* fix ring id

* add point_filters params

* add default point_filter(no reduction)

* one third all OT128

* set default no reduction

* fix

Signed-off-by: Tomohito ANDO <tomohito.ando@tier4.jp>

---------

Signed-off-by: Tomohito ANDO <tomohito.ando@tier4.jp>
Co-authored-by: Tomohito ANDO <tomohito.ando@tier4.jp>
---
 .../config/point_filters_full.param.yaml      |  5 +
 .../config/point_filters_one_half.param.yaml  | 71 ++++++++++++++
 .../config/point_filters_one_third.param.yaml | 92 +++++++++++++++++++
 .../config/point_filters_two_third.param.yaml | 50 ++++++++++
 .../launch/hesai_OT128.launch.xml             |  2 +
 .../launch/hesai_QT128.launch.xml             |  2 +
 aip_x2_gen2_launch/launch/lidar.launch.xml    |  4 +
 .../launch/nebula_node_container.launch.py    |  5 +-
 8 files changed, 229 insertions(+), 2 deletions(-)
 create mode 100644 aip_x2_gen2_launch/config/point_filters_full.param.yaml
 create mode 100644 aip_x2_gen2_launch/config/point_filters_one_half.param.yaml
 create mode 100644 aip_x2_gen2_launch/config/point_filters_one_third.param.yaml
 create mode 100644 aip_x2_gen2_launch/config/point_filters_two_third.param.yaml

diff --git a/aip_x2_gen2_launch/config/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters_full.param.yaml
new file mode 100644
index 00000000..7ff58606
--- /dev/null
+++ b/aip_x2_gen2_launch/config/point_filters_full.param.yaml
@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    point_filters: |
+      {
+      }
diff --git a/aip_x2_gen2_launch/config/point_filters_one_half.param.yaml b/aip_x2_gen2_launch/config/point_filters_one_half.param.yaml
new file mode 100644
index 00000000..42db0ed7
--- /dev/null
+++ b/aip_x2_gen2_launch/config/point_filters_one_half.param.yaml
@@ -0,0 +1,71 @@
+/**:
+  ros__parameters:
+    point_filters: |
+      {
+        "ring_section_filter": [
+        [1, 0, 360],
+        [3, 0, 360],
+        [5, 0, 360],
+        [7, 0, 360],
+        [9, 0, 360],
+        [11, 0, 360],
+        [13, 0, 360],
+        [15, 0, 360],
+        [17, 0, 360],
+        [19, 0, 360],
+        [21, 0, 360],
+        [23, 0, 360],
+        [25, 0, 360],
+        [27, 0, 360],
+        [29, 0, 360],
+        [31, 0, 360],
+        [33, 0, 360],
+        [35, 0, 360],
+        [37, 0, 360],
+        [39, 0, 360],
+        [41, 0, 360],
+        [43, 0, 360],
+        [45, 0, 360],
+        [47, 0, 360],
+        [49, 0, 360],
+        [51, 0, 360],
+        [53, 0, 360],
+        [55, 0, 360],
+        [57, 0, 360],
+        [59, 0, 360],
+        [61, 0, 360],
+        [63, 0, 360],
+        [65, 0, 360],
+        [67, 0, 360],
+        [69, 0, 360],
+        [71, 0, 360],
+        [73, 0, 360],
+        [75, 0, 360],
+        [77, 0, 360],
+        [79, 0, 360],
+        [81, 0, 360],
+        [83, 0, 360],
+        [85, 0, 360],
+        [87, 0, 360],
+        [89, 0, 360],
+        [91, 0, 360],
+        [93, 0, 360],
+        [95, 0, 360],
+        [97, 0, 360],
+        [99, 0, 360],
+        [101, 0, 360],
+        [103, 0, 360],
+        [105, 0, 360],
+        [107, 0, 360],
+        [109, 0, 360],
+        [111, 0, 360],
+        [113, 0, 360],
+        [115, 0, 360],
+        [117, 0, 360],
+        [119, 0, 360],
+        [121, 0, 360],
+        [123, 0, 360],
+        [125, 0, 360],
+        [127, 0, 360]
+        ]
+      }
diff --git a/aip_x2_gen2_launch/config/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters_one_third.param.yaml
new file mode 100644
index 00000000..075381cc
--- /dev/null
+++ b/aip_x2_gen2_launch/config/point_filters_one_third.param.yaml
@@ -0,0 +1,92 @@
+/**:
+  ros__parameters:
+    point_filters: |
+      {
+        "ring_section_filter": [
+        [1, 0, 360],
+        [2, 0, 360],
+        [4, 0, 360],
+        [5, 0, 360],
+        [7, 0, 360],
+        [8, 0, 360],
+        [10, 0, 360],
+        [11, 0, 360],
+        [13, 0, 360],
+        [14, 0, 360],
+        [16, 0, 360],
+        [17, 0, 360],
+        [19, 0, 360],
+        [20, 0, 360],
+        [22, 0, 360],
+        [23, 0, 360],
+        [25, 0, 360],
+        [26, 0, 360],
+        [28, 0, 360],
+        [29, 0, 360],
+        [31, 0, 360],
+        [32, 0, 360],
+        [34, 0, 360],
+        [35, 0, 360],
+        [37, 0, 360],
+        [38, 0, 360],
+        [40, 0, 360],
+        [41, 0, 360],
+        [43, 0, 360],
+        [44, 0, 360],
+        [46, 0, 360],
+        [47, 0, 360],
+        [49, 0, 360],
+        [50, 0, 360],
+        [52, 0, 360],
+        [53, 0, 360],
+        [55, 0, 360],
+        [56, 0, 360],
+        [58, 0, 360],
+        [59, 0, 360],
+        [61, 0, 360],
+        [62, 0, 360],
+        [64, 0, 360],
+        [65, 0, 360],
+        [67, 0, 360],
+        [68, 0, 360],
+        [70, 0, 360],
+        [71, 0, 360],
+        [73, 0, 360],
+        [74, 0, 360],
+        [76, 0, 360],
+        [77, 0, 360],
+        [79, 0, 360],
+        [80, 0, 360],
+        [82, 0, 360],
+        [83, 0, 360],
+        [85, 0, 360],
+        [86, 0, 360],
+        [88, 0, 360],
+        [89, 0, 360],
+        [91, 0, 360],
+        [92, 0, 360],
+        [94, 0, 360],
+        [95, 0, 360],
+        [97, 0, 360],
+        [98, 0, 360],
+        [100, 0, 360],
+        [101, 0, 360],
+        [103, 0, 360],
+        [104, 0, 360],
+        [106, 0, 360],
+        [107, 0, 360],
+        [109, 0, 360],
+        [110, 0, 360],
+        [112, 0, 360],
+        [113, 0, 360],
+        [115, 0, 360],
+        [116, 0, 360],
+        [118, 0, 360],
+        [119, 0, 360],
+        [121, 0, 360],
+        [122, 0, 360],
+        [124, 0, 360],
+        [125, 0, 360],
+        [127, 0, 360]
+        ]
+      }
diff --git a/aip_x2_gen2_launch/config/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters_two_third.param.yaml
new file mode 100644
index 00000000..45a57f55
--- /dev/null
+++ b/aip_x2_gen2_launch/config/point_filters_two_third.param.yaml
@@ -0,0 +1,50 @@
+/**:
+  ros__parameters:
+    point_filters: |
+      {
+        "ring_section_filter": [
+        [0, 0, 360],
+        [3, 0, 360],
+        [6, 0, 360],
+        [9, 0, 360],
+        [12, 0, 360],
+        [15, 0, 360],
+        [18, 0, 360],
+        [21, 0, 360],
+        [24, 0, 360],
+        [27, 0, 360],
+        [30, 0, 360],
+        [33, 0, 360],
+        [36, 0, 360],
+        [39, 0, 360],
+        [42, 0, 360],
+        [45, 0, 360],
+        [48, 0, 360],
+        [51, 0, 360],
+        [54, 0, 360],
+        [57, 0, 360],
+        [60, 0, 360],
+        [63, 0, 360],
+        [66, 0, 360],
+        [69, 0, 360],
+        [72, 0, 360],
+        [75, 0, 360],
+        [78, 0, 360],
+        [81, 0, 360],
+        [84, 0, 360],
+        [87, 0, 360],
+        [90, 0, 360],
+        [93, 0, 360],
+        [96, 0, 360],
+        [99, 0, 360],
+        [102, 0, 360],
+        [105, 0, 360],
+        [108, 0, 360],
+        [111, 0, 360],
+        [114, 0, 360],
+        [117, 0, 360],
+        [120, 0, 360],
+        [123, 0, 360],
+        [126, 0, 360]
+        ]
+      }
diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
index cfc4f138..6315c1b3 100644
--- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
+++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
@@ -30,6 +30,7 @@
   <arg name="dual_return_filter_param_file" default="$(var dual_return_filter_param_file)" />
   <arg name="vehicle_mirror_param_file" default="$(var vehicle_mirror_param_file)" />
   <arg name="blockage_diagnostics_param_file" default="$(find-pkg-share aip_x2_gen2_launch)/config/blockage_diagnostics_param_file.yaml" />
+  <arg name="point_filters_param_file" default="$(find-pkg-share aip_x2_gen2_launch)/config/point_filters_full.param.yaml" />
 
   <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_hw" value="$(var launch_driver)"/>
@@ -64,6 +65,7 @@
     <arg name="calibration_file" value="$(var calibration_file)"/>
     <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)"/>
     <arg name="blockage_diagnostics_param_file" value="$(var blockage_diagnostics_param_file)"/>
+    <arg name="point_filters_param_file" value="$(var point_filters_param_file)"/>
 
   </include>
 </launch>
diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
index 9f48bfae..8b0276d5 100644
--- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
+++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
@@ -31,6 +31,7 @@
   <arg name="dual_return_filter_param_file" default="$(var dual_return_filter_param_file)" />
   <arg name="vehicle_mirror_param_file" default="$(var vehicle_mirror_param_file)" />
   <arg name="blockage_diagnostics_param_file" default="$(find-pkg-share aip_x2_gen2_launch)/config/blockage_diagnostics_param_file.yaml" />
+  <arg name="point_filters_param_file" default="$(find-pkg-share aip_x2_gen2_launch)/config/point_filters_full.param.yaml" />
 
   <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_hw" value="$(var launch_driver)"/>
@@ -64,5 +65,6 @@
     <arg name="calibration_file" value="$(var calibration_file)"/>
     <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)"/>
     <arg name="blockage_diagnostics_param_file" value="$(var blockage_diagnostics_param_file)"/>
+    <arg name="point_filters_param_file" value="$(var point_filters_param_file)"/>
   </include>
 </launch>
diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml
index 8f1c219a..6c68f439 100644
--- a/aip_x2_gen2_launch/launch/lidar.launch.xml
+++ b/aip_x2_gen2_launch/launch/lidar.launch.xml
@@ -55,6 +55,7 @@
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/rear_upper.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
+        <arg name="point_filters_param_file" value="$(find-pkg-share aip_x2_gen2_launch)/config/point_filters_full.param.yaml" />
       </include>
     </group>
 
@@ -88,6 +89,7 @@
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/left_upper.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
+        <arg name="point_filters_param_file" value="$(find-pkg-share aip_x2_gen2_launch)/config/point_filters_full.param.yaml" />
       </include>
     </group>
 
@@ -188,6 +190,7 @@
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_upper.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
+        <arg name="point_filters_param_file" value="$(find-pkg-share aip_x2_gen2_launch)/config/point_filters_full.param.yaml" />
       </include>
     </group>
 
@@ -221,6 +224,7 @@
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/right_upper.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
+        <arg name="point_filters_param_file" value="$(find-pkg-share aip_x2_gen2_launch)/config/point_filters_full.param.yaml" />
       </include>
     </group>
 
diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
index 49ec3779..f76911eb 100644
--- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
+++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
@@ -94,7 +94,6 @@ def str2vector(string):
         parameters=[
             {
                 "sensor_model": sensor_model,
-                "point_filters": "{}",
                 **create_parameter_dict(
                     "host_ip",
                     "sensor_ip",
@@ -123,7 +122,8 @@ def str2vector(string):
                 ),
                 "retry_hw": True,
             },
-        ],
+        ]
+        + [load_composable_node_param("point_filters_param_file")],
         remappings=[
             # ("aw_points", "pointcloud_raw"),
             ("pandar_points", "pointcloud_raw_ex"),
@@ -367,6 +367,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("min_azimuth_deg", "135.0")
     add_launch_arg("max_azimuth_deg", "225.0")
     add_launch_arg("enable_blockage_diag", "true")
+    add_launch_arg("point_filters_param_file")
 
     add_launch_arg("calibration_file", "")