From 803c445fe7bd48e33d2107f97239617a256f042d Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 1 Oct 2024 12:07:14 +0900 Subject: [PATCH] feat(aip_launcher): ring outlier filter node load from param file (#305) * feat: ring outlier filter node load from param file Signed-off-by: vividf * chore: remove distortion related in order to seperate PRs Signed-off-by: vividf * chore: add param file Signed-off-by: vividf --------- Signed-off-by: vividf --- ...ile.yaml => blockage_diagnostics.param.yaml} | 0 .../config/ring_outlier_filter_node.param.yaml | 14 ++++++++++++++ .../launch/pandar_node_container.launch.py | 17 +++++++++++------ .../config/ring_outlier_filter_node.param.yaml | 14 ++++++++++++++ .../launch/nebula_node_container.launch.py | 4 ++++ 5 files changed, 43 insertions(+), 6 deletions(-) rename aip_x2_launch/config/{blockage_diagnostics_param_file.yaml => blockage_diagnostics.param.yaml} (100%) create mode 100644 aip_x2_launch/config/ring_outlier_filter_node.param.yaml create mode 100644 common_sensor_launch/config/ring_outlier_filter_node.param.yaml diff --git a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics.param.yaml similarity index 100% rename from aip_x2_launch/config/blockage_diagnostics_param_file.yaml rename to aip_x2_launch/config/blockage_diagnostics.param.yaml diff --git a/aip_x2_launch/config/ring_outlier_filter_node.param.yaml b/aip_x2_launch/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 00000000..76bf6895 --- /dev/null +++ b/aip_x2_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/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index d1e79bb7..6fad838f 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -209,11 +209,9 @@ def create_parameter_dict(*args): # Ring Outlier Filter is the last component in the pipeline, so control the output frame here if LaunchConfiguration("output_as_sensor_frame").perform(context): - ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} else: - ring_outlier_filter_parameters = { - "output_frame": "" - } # keep the output frame as the input frame + ring_outlier_output_frame = {"output_frame": ""} # keep the output frame as the input frame ring_outlier_filter_component = ComposableNode( package="autoware_pointcloud_preprocessor", plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent", @@ -222,7 +220,10 @@ def create_parameter_dict(*args): ("input", "rectified/pointcloud_ex"), ("output", "pointcloud_before_sync"), ], - parameters=[ring_outlier_filter_parameters], + parameters=[ + load_composable_node_param("ring_outlier_filter_node_param_file"), + ring_outlier_output_frame, + ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -342,7 +343,11 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("horizontal_resolution", "0.4") add_launch_arg( "blockage_diagnostics_param_file", - [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"], + [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics.param.yaml"], + ) + add_launch_arg( + "ring_outlier_filter_node_param_file", + [FindPackageShare("aip_x2_launch"), "/config/ring_outlier_filter_node.param.yaml"], ) add_launch_arg( "distortion_corrector_node_param_file", diff --git a/common_sensor_launch/config/ring_outlier_filter_node.param.yaml b/common_sensor_launch/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 00000000..76bf6895 --- /dev/null +++ b/common_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_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index e9660646..bcd5ec5c 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -382,6 +382,10 @@ def add_launch_arg(name: str, default_value=None, description=None): "distortion_corrector_node_param_file", [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], ) + add_launch_arg( + "ring_outlier_filter_node_param_file", + [FindPackageShare("common_sensor_launch"), "/config/ring_outlier_filter_node.param.yaml"], + ) set_container_executable = SetLaunchConfiguration( "container_executable",