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",