From c9d287dbe5a4be1e949bf99bbc78e651e8db390e Mon Sep 17 00:00:00 2001
From: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Date: Wed, 2 Oct 2024 10:04:21 +0900
Subject: [PATCH] Fixed ring_outlier_filter_param

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
---
 common_awsim_sensor_launch/CMakeLists.txt     |  1 +
 .../ring_outlier_filter_node.param.yaml       | 14 +++++++++
 .../launch/velodyne_node_container.launch.py  | 29 +++++++++++++++++++
 3 files changed, 44 insertions(+)
 create mode 100644 common_awsim_sensor_launch/config/ring_outlier_filter_node.param.yaml

diff --git a/common_awsim_sensor_launch/CMakeLists.txt b/common_awsim_sensor_launch/CMakeLists.txt
index 9d66934..8a9a6a0 100644
--- a/common_awsim_sensor_launch/CMakeLists.txt
+++ b/common_awsim_sensor_launch/CMakeLists.txt
@@ -10,5 +10,6 @@ if(BUILD_TESTING)
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE
+  config
   launch
 )
diff --git a/common_awsim_sensor_launch/config/ring_outlier_filter_node.param.yaml b/common_awsim_sensor_launch/config/ring_outlier_filter_node.param.yaml
new file mode 100644
index 0000000..76bf689
--- /dev/null
+++ b/common_awsim_sensor_launch/config/ring_outlier_filter_node.param.yaml
@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    distance_ratio: 1.03
+    object_length_threshold: 0.1
+    num_points_threshold: 4
+    max_rings_num: 128
+    max_points_num_per_ring: 4000
+    publish_outlier_pointcloud: false
+    min_azimuth_deg: 0.0
+    max_azimuth_deg: 360.0
+    max_distance: 12.0
+    vertical_bins: 128
+    horizontal_bins: 36
+    noise_threshold: 2
diff --git a/common_awsim_sensor_launch/launch/velodyne_node_container.launch.py b/common_awsim_sensor_launch/launch/velodyne_node_container.launch.py
index 87565eb..fb08701 100644
--- a/common_awsim_sensor_launch/launch/velodyne_node_container.launch.py
+++ b/common_awsim_sensor_launch/launch/velodyne_node_container.launch.py
@@ -22,7 +22,10 @@
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
 import yaml
+from ament_index_python.packages import get_package_share_directory
+from pathlib import Path
 
 
 def get_vehicle_info(context):
@@ -106,6 +109,20 @@ def create_parameter_dict(*args):
         )
     )
 
+    ring_outlier_filter_node_param = ParameterFile(
+        param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(
+            context
+        ),
+        allow_substs=True,
+    )
+
+    # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+    if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
+        ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
+    else:
+        # keep the output frame as the input frame
+        ring_outlier_output_frame = {"output_frame": ""}
+
     nodes.append(
         ComposableNode(
             package="autoware_pointcloud_preprocessor",
@@ -115,6 +132,7 @@ def create_parameter_dict(*args):
                 ("input", "rectified/pointcloud_ex"),
                 ("output", "pointcloud"),
             ],
+            parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
     )
@@ -179,6 +197,10 @@ def add_launch_arg(name: str, default_value=None, description=None):
             DeclareLaunchArgument(name, default_value=default_value, description=description)
         )
 
+    common_sensor_share_dir = Path(
+        get_package_share_directory("common_awsim_sensor_launch")
+    )
+
     add_launch_arg("base_frame", "base_link", "base frame id")
     add_launch_arg("container_name", "velodyne_composable_node_container", "container name")
     add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox")
@@ -188,6 +210,13 @@ def add_launch_arg(name: str, default_value=None, description=None):
     )
     add_launch_arg("use_multithread", "False", "use multithread")
     add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
+    add_launch_arg("output_as_sensor_frame", "False", "output final pointcloud in sensor frame")
+    add_launch_arg("frame_id", "base_link", "frame id")
+    add_launch_arg(
+        "ring_outlier_filter_node_param_path",
+        str(common_sensor_share_dir / "config" / "ring_outlier_filter_node.param.yaml"),
+        description="path to parameter file of ring outlier filter node",
+    )
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",