From d7b7b1733b07ef9183a4555b835aaeb44b328bf5 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 4 Feb 2025 07:55:49 +0900 Subject: [PATCH] chore(aip_x2_gen2_launch): use parameter file for ring_outlier_filter (#385) Signed-off-by: Tomohito Ando --- .../launch/nebula_node_container.launch.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) 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 6497123f..835af5e7 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -26,6 +26,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 from launch_ros.substitutions import FindPackageShare import yaml @@ -161,6 +162,18 @@ def str2vector(string): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + ring_outlier_filter_node_param = ParameterFile( + param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").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": ""} + ring_outlier_filter_component = ComposableNode( package="autoware_pointcloud_preprocessor", plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent", @@ -169,6 +182,7 @@ def str2vector(string): ("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")}], ) @@ -298,6 +312,10 @@ def add_launch_arg(name: str, default_value=None, description=None): "blockage_diagnostics_param_file", [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics.param.yaml"], ) + add_launch_arg( + "ring_outlier_filter_node_param_file", + [FindPackageShare("common_sensor_launch"), "/config/ring_outlier_filter_node.param.yaml"], + ) add_launch_arg( "distortion_corrector_node_param_file", [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], @@ -312,6 +330,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("point_filters_param_file") add_launch_arg("calibration_file", "") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") set_container_executable = SetLaunchConfiguration( "container_executable",