From d61de9d83cf0d4bd1b148306ef6fd29f9b92e6d4 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 22 Oct 2024 19:14:52 +0900 Subject: [PATCH 1/2] feature: filter load from parameter file Signed-off-by: vividf --- .../config/filter.param.yaml | 8 ++++++++ .../launch/velodyne_node_container.launch.py | 19 ++++++++++++++++--- 2 files changed, 24 insertions(+), 3 deletions(-) create mode 100644 common_awsim_labs_sensor_launch/config/filter.param.yaml diff --git a/common_awsim_labs_sensor_launch/config/filter.param.yaml b/common_awsim_labs_sensor_launch/config/filter.param.yaml new file mode 100644 index 0000000..2377bd0 --- /dev/null +++ b/common_awsim_labs_sensor_launch/config/filter.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + input_frame: "" + output_frame: "" + max_queue_size: 5 + use_indices: false + latched_indices: false + approximate_sync: false \ No newline at end of file diff --git a/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py b/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py index 0f5993c..5037122 100644 --- a/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py +++ b/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py @@ -60,6 +60,10 @@ def create_parameter_dict(*args): return result # Pointcloud preprocessor parameters + filter_param = ParameterFile( + param_file=LaunchConfiguration("filter_param_path").perform(context), + allow_substs=True, + ) distortion_corrector_node_param = ParameterFile( param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context), allow_substs=True, @@ -98,7 +102,7 @@ def create_parameter_dict(*args): ("input", "pointcloud_raw_ex"), ("output", "self_cropped/pointcloud_ex"), ], - parameters=[cropbox_parameters], + parameters=[filter_param, cropbox_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -120,7 +124,7 @@ def create_parameter_dict(*args): ("input", "self_cropped/pointcloud_ex"), ("output", "mirror_cropped/pointcloud_ex"), ], - parameters=[cropbox_parameters], + parameters=[filter_param, cropbox_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -154,7 +158,7 @@ def create_parameter_dict(*args): ("input", "rectified/pointcloud_ex"), ("output", "pointcloud_before_sync"), ], - parameters=[ring_outlier_filter_node_param], + parameters=[filter_param, ring_outlier_filter_node_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -195,6 +199,15 @@ def add_launch_arg(name: str, default_value=None, description=None): 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( + "filter_param_path", + os.path.join( + common_sensor_share_dir, + "config", + "filter.param.yaml", + ), + description="path to parameter file of filter", + ) add_launch_arg( "distortion_correction_node_param_path", os.path.join( From d0b684702cccc8473c7c2fb949f0abf964a122d8 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 22 Oct 2024 19:59:14 +0900 Subject: [PATCH 2/2] chore: fix yaml Signed-off-by: vividf --- common_awsim_labs_sensor_launch/config/filter.param.yaml | 2 +- .../launch/velodyne_node_container.launch.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/common_awsim_labs_sensor_launch/config/filter.param.yaml b/common_awsim_labs_sensor_launch/config/filter.param.yaml index 2377bd0..abd3968 100644 --- a/common_awsim_labs_sensor_launch/config/filter.param.yaml +++ b/common_awsim_labs_sensor_launch/config/filter.param.yaml @@ -5,4 +5,4 @@ max_queue_size: 5 use_indices: false latched_indices: false - approximate_sync: false \ No newline at end of file + approximate_sync: false diff --git a/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py b/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py index 5037122..67abbba 100644 --- a/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py +++ b/common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py @@ -59,6 +59,7 @@ def create_parameter_dict(*args): result[x] = LaunchConfiguration(x) return result + # Pointcloud preprocessor parameters filter_param = ParameterFile( param_file=LaunchConfiguration("filter_param_path").perform(context),