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 ab32e0fe..56fc9dbd 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 417d4972..279bf9f2 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/CMakeLists.txt b/common_sensor_launch/CMakeLists.txt index 60a50fc2..5e719ce7 100644 --- a/common_sensor_launch/CMakeLists.txt +++ b/common_sensor_launch/CMakeLists.txt @@ -12,4 +12,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE config launch + config ) 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..c6ce8e11 --- /dev/null +++ b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,12 @@ +/**: + 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 + blockage_kernel: 10 diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index c14c1390..ecd21d79 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -25,6 +25,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 @@ -54,14 +55,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: @@ -151,7 +149,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"] @@ -239,13 +237,49 @@ 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": [ + float(context.perform_substitution(LaunchConfiguration("cloud_min_angle"))), + float(context.perform_substitution(LaunchConfiguration("cloud_max_angle"))), + ], + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + "max_distance_range": LaunchConfiguration("max_range"), + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), + } + ] + + [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) + else LaunchConfiguration("container_name") + ) + driver_component_loader = LoadComposableNodes( composable_node_descriptions=[driver_component], target_container=container, condition=IfCondition(LaunchConfiguration("launch_driver")), ) + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=target_container, + condition=IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) - return [container, driver_component_loader] + return [container, driver_component_loader, blockage_diag_loader] def generate_launch_description(): @@ -284,6 +318,16 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") + add_launch_arg("enable_blockage_diag", "true") + add_launch_arg("horizontal_ring_id", "64") + add_launch_arg("vertical_bins", "128") + add_launch_arg("is_channel_order_top2down", "true") + add_launch_arg("horizontal_resolution", "0.4") + 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", diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml index faa32305..a105b0a2 100644 --- a/common_sensor_launch/launch/velodyne_VLP16.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP16.launch.xml @@ -16,6 +16,11 @@ + + + + + @@ -34,6 +39,11 @@ + + + + + diff --git a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml index c3db6428..3544d16e 100644 --- a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml @@ -16,6 +16,11 @@ + + + + + @@ -34,6 +39,11 @@ + + + + + diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml index f1d32469..01c8affa 100644 --- a/common_sensor_launch/launch/velodyne_VLS128.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLS128.launch.xml @@ -16,6 +16,11 @@ + + + + + @@ -34,6 +39,11 @@ + + + + +