feat(ring_filter): add ring_filter to launcher

Signed-off-by: Takayuki AKAMINE <[email protected]>
tier4 · Sep 13, 2024 · 5ebf052 · 5ebf052
1 parent 604a372
commit 5ebf052
Show file tree

Hide file tree

Showing 4 changed files with 73 additions and 1 deletion.
diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
@@ -31,6 +31,11 @@
   <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" />
 
+  <!-- ring filter params -->
+  <arg name="enable_ring_filter" default="true"/>
+  <arg name="ring_interval" default="2"/>
+  <arg name="remove_interval_ring" default="true"/>
+
   <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
     <arg name="sensor_model" value="$(var model)"/>
@@ -64,6 +69,9 @@
     <arg name="calibration" value="$(var calibration)"/>
     <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="enable_ring_filter" value="$(var enable_ring_filter)"/>
+    <arg name="ring_interval" value="$(var ring_interval)"/>
+    <arg name="remove_interval_ring" value="$(var remove_interval_ring)"/>
 
   </include>
 </launch>
diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
@@ -32,6 +32,11 @@
   <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" />
 
+
+  <!-- ring filter params -->
+  <arg name="enable_ring_filter" default="false"/>
+  <arg name="ring_interval" default="2"/>
+
   <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
     <arg name="sensor_model" value="$(var model)"/>
@@ -64,5 +69,9 @@
     <arg name="calibration" value="$(var calibration)"/>
     <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="enable_ring_filter" value="$(var enable_ring_filter)"/>
+    <!-- <arg name="ring_interval" value="$(var ring_interval)"/> -->
+
   </include>
 </launch>
diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml
@@ -13,6 +13,14 @@
   <arg name="dual_return_filter_param_file" default="$(find-pkg-share aip_x2_gen2_launch)/config/dual_return_filter.param.yaml"/>
   <arg name="enable_blockage_diag" default="false"/>
 
+  <!-- use 2/3 of points -->
+  <arg name="ring_interval" default="3"/>
+  <arg name="remove_interval_ring" default="true"/>
+
+  <!-- use 1/3 of points -->
+  <!-- <arg name="ring_interval" default="3"/>
+  <arg name="remove_interval_ring" default="false"/> -->
+
   <group>
     <push-ros-namespace namespace="lidar" />
 
@@ -44,6 +52,9 @@
         <arg name="ptp_domain" value="0"/>
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
+        <arg name="enable_ring_filter" value="true" />
+        <arg name="ring_interval" value="$(var ring_interval)" />
+        <arg name="remove_interval_ring" value="$(var remove_interval_ring)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
 
         <arg name="distance_range" value="[0.5, 200.0]"/>
@@ -80,6 +91,9 @@
         <arg name="ptp_domain" value="0"/>
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
+        <arg name="enable_ring_filter" value="false" />
+        <arg name="ring_interval" value="$(var ring_interval)" />
+        <arg name="remove_interval_ring" value="$(var remove_interval_ring)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
 
         <arg name="distance_range" value="[0.5, 200.0]"/>
@@ -189,6 +203,9 @@
         <arg name="ptp_domain" value="0"/>
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
+        <arg name="enable_ring_filter" value="true" />
+        <arg name="ring_interval" value="$(var ring_interval)" />
+        <arg name="remove_interval_ring" value="$(var remove_interval_ring)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
 
         <arg name="distance_range" value="[0.5, 200.0]"/>
@@ -225,6 +242,9 @@
         <arg name="ptp_domain" value="0"/>
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
+        <arg name="enable_ring_filter" value="false" />
+        <arg name="ring_interval" value="$(var ring_interval)" />
+        <arg name="remove_interval_ring" value="$(var remove_interval_ring)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
 
         <arg name="distance_range" value="[0.5, 200.0]"/>

diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
@@ -161,7 +161,13 @@ def str2vector(string):
         plugin="pointcloud_preprocessor::CropBoxFilterComponent",
         name="crop_box_filter_self",
         remappings=[
-            ("input", "pointcloud_raw_ex"),
+            ("input",
+              (
+                  "ring_filtered/pointcloud"
+                  if LaunchConfiguration("enable_ring_filter").perform(context) == "true"
+                  else "pointcloud_raw_ex"
+              ),
+            ),
             ("output", "self_cropped/pointcloud_ex"),
         ],
         parameters=[cropbox_parameters],
@@ -280,6 +286,23 @@ def str2vector(string):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    ring_filter_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin="pointcloud_preprocessor::RingFilterComponent",
+        name="ring_filter",
+        remappings=[
+            ("input", "pointcloud_raw_ex"),
+            ("output", "ring_filtered/pointcloud"),
+        ],
+        parameters=[
+            {
+                "ring_interval": LaunchConfiguration("ring_interval"),
+                "remove_interval_ring": LaunchConfiguration("remove_interval_ring"),
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
     container = ComposableNodeContainer(
         name="nebula_node_container",
         namespace="pointcloud_preprocessor",
@@ -313,11 +336,18 @@ def str2vector(string):
         condition=launch.conditions.IfCondition(LaunchConfiguration("enable_blockage_diag")),
     )
 
+    ring_filter_loader = LoadComposableNodes(
+        composable_node_descriptions=[ring_filter_component],
+        target_container=container,
+        condition=launch.conditions.IfCondition(LaunchConfiguration("enable_ring_filter")),
+    )
+
     return [
         container,
         # ring_outlier_filter_loader,
         dual_return_filter_loader,
         blockage_diag_loader,
+        ring_filter_loader,
     ]
 
 
@@ -382,6 +412,11 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("max_azimuth_deg", "225.0")
     add_launch_arg("enable_blockage_diag", "true")
 
+    # ring filter params
+    add_launch_arg("enable_ring_filter", "true")
+    add_launch_arg("ring_interval", "2")
+    add_launch_arg("remove_interval_ring", "true")
+
     # TODO: remove
     add_launch_arg("scan_phase", "0.0")