From 94295071fa429b87e71590013e2de08200add271 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Wed, 17 Jul 2024 19:22:32 +0900 Subject: [PATCH] set param for dual_return_filter and blockage_diag --- aip_x2_launch/launch/lidar.launch.xml | 148 +++++++++--------- .../launch/nebula_node_container.launch.py | 117 ++++++++++++-- 2 files changed, 177 insertions(+), 88 deletions(-) diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 7d761f44..a889aab6 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -7,7 +7,7 @@ - + @@ -24,22 +24,22 @@ - - - + + + - - - - - + + + + + - + @@ -57,22 +57,22 @@ - - - + + + - - - - - + + + + + - + @@ -90,22 +90,22 @@ - - - + + + - - - - - + + + + + - + @@ -123,22 +123,22 @@ - - - + + + - - - - - + + + + + - + @@ -156,22 +156,22 @@ - - - + + + - - - - - + + + + + - + @@ -189,22 +189,22 @@ - - - + + + - - - - - + + + + + - + @@ -222,22 +222,22 @@ - - - + + + - - - - - + + + + + - + @@ -255,22 +255,22 @@ - - - - + + + + - - - - - + + + + + - + diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py index 7fb66c08..3c254bc3 100644 --- a/aip_x2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -20,11 +20,14 @@ from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration 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 @@ -35,6 +38,8 @@ def get_lidar_make(sensor_name): return "Velodyne", ".yaml" return "unrecognized_sensor_model" +def str2vector(string): + return [float(x) for x in string.strip("[]").split(",")] def get_vehicle_info(context): # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support @@ -247,19 +252,6 @@ def create_parameter_dict(*args): ) ) - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud_before_sync"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - # set container to run all required components in the same process container = ComposableNodeContainer( name=LaunchConfiguration("container_name"), @@ -270,6 +262,44 @@ def create_parameter_dict(*args): output="both", ) + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here + if LaunchConfiguration("output_as_sensor_frame").perform(context): + ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + else: + ring_outlier_filter_parameters = { + "output_frame": "" + } # keep the output frame as the input frame + ring_outlier_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud_before_sync"), + ], + parameters=[ring_outlier_filter_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + dual_return_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DualReturnOutlierFilterComponent", + name="dual_return_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud_before_sync"), + ], + parameters=[ + { + "vertical_bins": LaunchConfiguration("vertical_bins"), + "min_azimuth_deg": LaunchConfiguration("min_azimuth_deg"), + "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"), + } + ] + + [load_composable_node_param("dual_return_filter_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + driver_component = ComposableNode( package="nebula_ros", plugin=sensor_make + "HwInterfaceRosWrapper", @@ -302,13 +332,59 @@ def create_parameter_dict(*args): ], ) + ring_outlier_filter_loader = LoadComposableNodes( + composable_node_descriptions=[ring_outlier_filter_component], + target_container=container, + condition=LaunchConfigurationNotEquals("return_mode", "Dual"), + ) + + dual_return_filter_loader = LoadComposableNodes( + composable_node_descriptions=[dual_return_filter_component], + target_container=container, + condition=LaunchConfigurationEquals("return_mode", "Dual"), + ) + driver_component_loader = LoadComposableNodes( composable_node_descriptions=[driver_component], target_container=container, condition=IfCondition(LaunchConfiguration("launch_driver")), ) - return [container, driver_component_loader] + blockage_diag_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_return_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"), + "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")}], + ) + + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) + + return [ + container, + driver_component_loader, + ring_outlier_filter_loader, + dual_return_filter_loader, + blockage_diag_loader, + ] def generate_launch_description(): @@ -327,6 +403,9 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") add_launch_arg("host_ip", "255.255.255.255", "host ip address") add_launch_arg("scan_phase", "0.0") + add_launch_arg("angle_range", "[270.0, 90.0]") + add_launch_arg("distance_range", "[0.1, 200.0]") + add_launch_arg("return_mode", "Dual") add_launch_arg("base_frame", "base_link", "base frame id") add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors") add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors") @@ -343,12 +422,22 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" ) + add_launch_arg( + "blockage_diagnostics_param_file", + [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"], + ) + add_launch_arg("horizontal_resolution", "0.4") + add_launch_arg("vertical_bins", "40") + add_launch_arg("horizontal_ring_id", "12") + add_launch_arg("enable_blockage_diag", "true") + add_launch_arg("is_channel_order_top2down", "True") add_launch_arg("diag_span", "1000") add_launch_arg("delay_monitor_ms", "2000") add_launch_arg("use_multithread", "True", "use multithread") add_launch_arg("use_intra_process", "True", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") add_launch_arg("calibration_file", "") + add_launch_arg("output_as_sensor_frame", "True") set_container_executable = SetLaunchConfiguration( "container_executable",