From a4f6d597c690aad263374f83bdf45811e858f901 Mon Sep 17 00:00:00 2001 From: Yusuke Mizoguchi <33048729+swiftfile@users.noreply.github.com> Date: Fri, 29 Sep 2023 01:48:07 +0900 Subject: [PATCH] feat: add params and diag about dust-detection (#116) * correspond to sensing/t4-wholedust-dev * add diag /sensing/lidar/~/dust ,add params * correspond to sensing/t4-wholedust-dev * add diag /sensing/lidar/~/dust ,add params * change initial parameter * chore: refactor Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Co-authored-by: badai-nguyen --- .../blockage_diagnostics_param_file.yaml | 11 ++++++ .../sensor_kit.param.yaml | 6 +++ .../launch/pandar_node_container.launch.py | 39 +++++++------------ 3 files changed, 30 insertions(+), 26 deletions(-) create mode 100644 aip_x2_launch/config/blockage_diagnostics_param_file.yaml diff --git a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 00000000..b8b62b50 --- /dev/null +++ b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.1 + blockage_count_threshold: 50 + blockage_buffering_frames: 2 + blockage_buffering_interval: 1 + dust_ratio_threshold: 0.2 + dust_count_threshold: 10 + dust_kernel_size: 2 + dust_buffering_frames: 10 + dust_buffering_interval: 1 diff --git a/aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 4809876b..6d83d1fc 100644 --- a/aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -61,6 +61,12 @@ contains: [": pandar_ptp"] timeout: 5.0 + dust: + type: diagnostic_aggregator/GenericAnalyzer + path: dust + contains: [": dust_validation"] + timeout: 1.0 + gnss: type: diagnostic_aggregator/AnalyzerGroup path: gnss diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index 898ec8a1..437e2dff 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -28,16 +28,10 @@ from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare import yaml -def get_dual_return_filter_info(context): - path = LaunchConfiguration("dual_return_filter_param_file").perform(context) - with open(path, "r") as f: - p = yaml.safe_load(f)["/**"]["ros__parameters"] - return p - - def get_pandar_monitor_info(): path = os.path.join( get_package_share_directory("pandar_monitor"), @@ -67,14 +61,11 @@ def get_vehicle_info(context): return p -def get_vehicle_mirror_info(context): - path = LaunchConfiguration("vehicle_mirror_param_file").perform(context) - with open(path, "r") as f: - p = yaml.safe_load(f)["/**"]["ros__parameters"] - return p - - def launch_setup(context, *args, **kwargs): + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + def create_parameter_dict(*args): result = {} for x in args: @@ -129,7 +120,6 @@ def create_parameter_dict(*args): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - dual_return_filter_info = get_dual_return_filter_info(context) cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -153,7 +143,7 @@ def create_parameter_dict(*args): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - mirror_info = get_vehicle_mirror_info(context) + mirror_info = load_composable_node_param("vehicle_mirror_param_file") right = mirror_info["right"] cropbox_parameters.update( min_x=right["min_longitudinal_offset"], @@ -238,7 +228,7 @@ def create_parameter_dict(*args): "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"), } ] - + [dual_return_filter_info], + + [load_composable_node_param("dual_return_filter_param_file")], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -254,14 +244,11 @@ def create_parameter_dict(*args): { "angle_range": LaunchConfiguration("angle_range"), "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), - "blockage_ratio_threshold": LaunchConfiguration("blockage_ratio_threshold"), "vertical_bins": LaunchConfiguration("vertical_bins"), "model": LaunchConfiguration("model"), - "blockage_count_threshold": LaunchConfiguration("blockage_count_threshold"), - "buffering_frames": LaunchConfiguration("buffering_frames"), - "buffering_interval": LaunchConfiguration("buffering_interval"), } - ], + ] + + [load_composable_node_param("blockage_diagnostics_param_file")], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -335,15 +322,15 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_frame", LaunchConfiguration("base_frame")) add_launch_arg("output_frame", LaunchConfiguration("base_frame")) add_launch_arg("dual_return_filter_param_file") + add_launch_arg( + "blockage_diagnostics_param_file", + [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"], + ) add_launch_arg("vehicle_mirror_param_file") add_launch_arg("use_multithread", "true") add_launch_arg("use_intra_process", "true") add_launch_arg("vertical_bins", "40") - add_launch_arg("blockage_ratio_threshold", "0.1") add_launch_arg("horizontal_ring_id", "12") - add_launch_arg("blockage_count_threshold", "50") - add_launch_arg("buffering_frames", "2") - add_launch_arg("buffering_interval", "1") add_launch_arg("min_azimuth_deg", "135.0") add_launch_arg("max_azimuth_deg", "225.0")