From 512e1c9500d636055cdde8934d8cfa3b5ef0baa8 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 21 Dec 2023 02:03:53 +0900 Subject: [PATCH] fix: enable blocakge diag for xx1 Signed-off-by: badai-nguyen --- .../sensor_kit.param.yaml | 9 ++++ aip_xx1_launch/launch/lidar.launch.xml | 21 ++++++++ .../blockage_diagnostics_param_file.yaml | 13 +++++ .../launch/velodyne_node_container.launch.py | 48 +++++++++++++++---- 4 files changed, 83 insertions(+), 8 deletions(-) create mode 100644 common_sensor_launch/config/blockage_diagnostics_param_file.yaml diff --git a/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 3b1b980e..02a09665 100644 --- a/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -18,6 +18,15 @@ type: diagnostic_aggregator/AnalyzerGroup path: lidar analyzers: + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + blockage: + type: diagnostic_aggregator/GenericAnalyzer + path: blockage + contains: [": blockage_validation"] + timeout: 1.0 velodyne: type: diagnostic_aggregator/AnalyzerGroup path: velodyne diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml index 4859302c..33830336 100644 --- a/aip_xx1_launch/launch/lidar.launch.xml +++ b/aip_xx1_launch/launch/lidar.launch.xml @@ -6,6 +6,7 @@ + @@ -22,6 +23,11 @@ + + + + + @@ -39,6 +45,11 @@ + + + + + @@ -56,6 +67,11 @@ + + + + + @@ -73,6 +89,11 @@ + + + + + diff --git a/common_sensor_launch/config/blockage_diagnostics_param_file.yaml b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 00000000..e8409a50 --- /dev/null +++ b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,13 @@ +/**: + 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 + distance_coefficient: 327.67 + horizontal_resolution: 0.4 diff --git a/common_sensor_launch/launch/velodyne_node_container.launch.py b/common_sensor_launch/launch/velodyne_node_container.launch.py index 55bedf17..2a38fc1c 100644 --- a/common_sensor_launch/launch/velodyne_node_container.launch.py +++ b/common_sensor_launch/launch/velodyne_node_container.launch.py @@ -22,6 +22,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare import yaml @@ -43,14 +44,12 @@ 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: @@ -114,7 +113,7 @@ def create_parameter_dict(*args): ) ) - mirror_info = get_vehicle_mirror_info(context) + mirror_info = load_composable_node_param("vehicle_mirror_param_file") cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"] cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"] cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"] @@ -205,6 +204,25 @@ def create_parameter_dict(*args): ], ) + blockage_diag_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_diag", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "blockage_diag/pointcloud"), + ], + parameters=[ + { + "angle_range":LaunchConfiguration("angle_range"), + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + } + ]+ [load_composable_node_param("blockage_diagnostics_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + target_container = ( container if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) @@ -216,8 +234,13 @@ def create_parameter_dict(*args): target_container=target_container, condition=IfCondition(LaunchConfiguration("launch_driver")), ) + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=target_container, + condition=IfCondition(LaunchConfiguration("launch_blockage_diag")), + ) - return [container, component_loader, driver_component_loader] + return [container, component_loader, driver_component_loader,blockage_diag_loader] def generate_launch_description(): @@ -262,6 +285,15 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_pointcloud_container", "false") add_launch_arg("container_name", "velodyne_node_container") + add_launch_arg("angle_range","[0.0, 360]") + add_launch_arg("horizontal_ring_id","64") + add_launch_arg("vertical_bins","128") + add_launch_arg("is_channel_order_top2down","true") + add_launch_arg( + "blockage_diagnostics_param_file", + [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics_param_file.yaml"], + ) + set_container_executable = SetLaunchConfiguration( "container_executable", "component_container",