Skip to content

Commit

Permalink
feat: ring outlier filter load from param file
Browse files Browse the repository at this point in the history
Signed-off-by: vividf <yihsiang.fang@tier4.jp>
  • Loading branch information
vividf committed Aug 29, 2024
1 parent b798c92 commit 0f6ec86
Show file tree
Hide file tree
Showing 2 changed files with 30 additions and 5 deletions.
14 changes: 14 additions & 0 deletions common_sensor_launch/config/ring_outlier_filter_node.param.yaml
Original file line number Diff line number Diff line change
@@ -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
21 changes: 16 additions & 5 deletions common_sensor_launch/launch/nebula_node_container.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,10 @@ def create_parameter_dict(*args):
param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
allow_substs=True,
)
ring_corrector_node_param = ParameterFile(
param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
allow_substs=True,
)

nodes = []

Expand Down Expand Up @@ -199,11 +203,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",
Expand All @@ -213,7 +215,7 @@ def create_parameter_dict(*args):
("input", "rectified/pointcloud_ex"),
("output", "pointcloud_before_sync"),
],
parameters=[ring_outlier_filter_parameters],
parameters=[ring_corrector_node_param, ring_outlier_output_frame],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
)
Expand Down Expand Up @@ -312,6 +314,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 distortion correction node",
)

set_container_executable = SetLaunchConfiguration(
"container_executable",
Expand Down

0 comments on commit 0f6ec86

Please sign in to comment.