From a5e7a2d9a285c664ca8ccc170004caa27ea60d8f 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:10 +0900 Subject: [PATCH] feat(common_sensor_launch): ring outlier filter load from param file (#103) * feat: ring outlier filter load from param file Signed-off-by: vividf * fix: fix variable name Signed-off-by: vividf * chore: fix description Signed-off-by: vividf --------- Signed-off-by: vividf --- .../ring_outlier_filter_node.param.yaml | 14 +++++++++++++ .../launch/nebula_node_container.launch.py | 21 ++++++++++++++----- 2 files changed, 30 insertions(+), 5 deletions(-) create mode 100644 common_sensor_launch/config/ring_outlier_filter_node.param.yaml 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 0000000..76bf689 --- /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 e885d92..d1f978a 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -95,6 +95,10 @@ def create_parameter_dict(*args): 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 = [] @@ -206,11 +210,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).lower() == "true": - 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 nodes.append( ComposableNode( package="autoware_pointcloud_preprocessor", @@ -220,7 +222,7 @@ def create_parameter_dict(*args): ("input", "rectified/pointcloud_ex"), ("output", "pointcloud_before_sync"), ], - parameters=[ring_outlier_filter_parameters], + parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -319,6 +321,15 @@ def add_launch_arg(name: str, default_value=None, description=None): ), 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",