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 @@
+
+
+
+
+