feat : ring outlier filter load from param file

Signed-off-by: A. Sena Yılmaz <[email protected]>

common_awsim_labs_sensor_launch/config/distortion_corrector_node.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    base_frame: base_link
+    use_imu: true
+    use_3d_distortion_correction: false

common_awsim_labs_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

common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py

@@ -11,7 +11,8 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-
+import os
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
@@ -22,6 +23,7 @@
 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
 
 
@@ -57,6 +59,16 @@ def create_parameter_dict(*args):
             result[x] = LaunchConfiguration(x)
         return result
 
+    # Pointcloud preprocessor parameters
+    distortion_corrector_node_param = ParameterFile(
+        param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
+        allow_substs=True,
+    )
+    ring_outlier_filter_node_param = ParameterFile(
+        param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
+        allow_substs=True,
+    )
+
     nodes = []
 
     cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
@@ -106,6 +118,10 @@ def create_parameter_dict(*args):
         )
     )
 
+    if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
+        ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
+    else:
+        ring_outlier_output_frame = {"output_frame": ""}  # keep the output frame as the input frame
     nodes.append(
         ComposableNode(
             package="autoware_pointcloud_preprocessor",
@@ -115,6 +131,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")}],
         )
     )
@@ -139,6 +156,7 @@ def create_parameter_dict(*args):
             ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"),
             ("~/output/pointcloud", "rectified/pointcloud_ex"),
         ],
+        parameters=[distortion_corrector_node_param],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -178,17 +196,38 @@ def add_launch_arg(name: str, default_value=None, description=None):
         launch_arguments.append(
             DeclareLaunchArgument(name, default_value=default_value, description=description)
         )
+
+    common_sensor_share_dir = get_package_share_directory("common_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")
     add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox")
+    add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
     add_launch_arg(
         "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
     )
+    add_launch_arg("frame_id", "lidar", "frame id")
     add_launch_arg("use_multithread", "False", "use multithread")
     add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
-
+    add_launch_arg(
+        "distortion_correction_node_param_path",
+        os.path.join(
+            common_sensor_share_dir,
+            "config",
+            "distortion_corrector_node.param.yaml",
+        ),
+        description="path to parameter file of distortion correction node",
+    )
+    add_launch_arg(
+        "ring_outlier_filter_node_param_path",
+        os.path.join(
+            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",
         "component_container",