diff --git a/localization_launch/config/crop_box_filter_measurement_range.param.yaml b/localization_launch/config/crop_box_filter_measurement_range.param.yaml new file mode 100644 index 0000000000000..ad5542315410e --- /dev/null +++ b/localization_launch/config/crop_box_filter_measurement_range.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + input_frame: "base_link" + output_frame: "base_link" + min_x: -60.0 + max_x: 60.0 + min_y: -60.0 + max_y: 60.0 + min_z: -30.0 + max_z: 50.0 + negative: False diff --git a/localization_launch/config/random_downsample_filter.param.yaml b/localization_launch/config/random_downsample_filter.param.yaml new file mode 100644 index 0000000000000..53be849e0af22 --- /dev/null +++ b/localization_launch/config/random_downsample_filter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + sample_num: 1500 diff --git a/localization_launch/config/voxel_grid_filter.param.yaml b/localization_launch/config/voxel_grid_filter.param.yaml new file mode 100644 index 0000000000000..51a7ee9d89b6b --- /dev/null +++ b/localization_launch/config/voxel_grid_filter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + voxel_size_x: 3.0 + voxel_size_y: 3.0 + voxel_size_z: 3.0 diff --git a/localization_launch/launch/util/util.launch.py b/localization_launch/launch/util/util.launch.py index bde66951be185..055eca0450c1a 100644 --- a/localization_launch/launch/util/util.launch.py +++ b/localization_launch/launch/util/util.launch.py @@ -13,13 +13,22 @@ # limitations under the License. import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.conditions import LaunchConfigurationNotEquals from launch.substitutions import LaunchConfiguration from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): + # https://github.com/ros2/launch_ros/issues/156 + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), 'r') as f: + return yaml.safe_load(f)['/**']['ros__parameters'] + crop_box_component = ComposableNode( package='pointcloud_preprocessor', plugin='pointcloud_preprocessor::CropBoxFilterComponent', @@ -29,17 +38,9 @@ def generate_launch_description(): ('output', LaunchConfiguration('output_measurement_range_sensor_points_topic')), ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': -60.0, - 'max_x': 60.0, - 'min_y': -60.0, - 'max_y': 60.0, - 'min_z': -30.0, - 'max_z': 50.0, - 'negative': False, - }], + parameters=[ + load_composable_node_param('crop_box_filter_measurement_range_param_path'), + ], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') }], @@ -54,11 +55,7 @@ def generate_launch_description(): ('output', LaunchConfiguration('output_voxel_grid_downsample_sensor_points_topic')), ], - parameters=[{ - 'voxel_size_x': 3.0, - 'voxel_size_y': 3.0, - 'voxel_size_z': 3.0, - }], + parameters=[load_composable_node_param('voxel_grid_downsample_filter_param_path')], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') }], @@ -73,9 +70,9 @@ def generate_launch_description(): ('output', LaunchConfiguration('output_downsample_sensor_points_topic')), ], - parameters=[{ - 'sample_num': 1500, - }], + parameters=[ + load_composable_node_param('random_downsample_filter_param_path') + ], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') }], @@ -90,4 +87,71 @@ def generate_launch_description(): composable_node_descriptions=composable_nodes, target_container=LaunchConfiguration('container'), ) - return launch.LaunchDescription([load_composable_nodes]) + + return [load_composable_nodes] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + arg = DeclareLaunchArgument(name, default_value=default_value, description=description) + launch_arguments.append(arg) + + add_launch_arg( + 'crop_box_filter_measurement_range_param_path', + [ + FindPackageShare('localization_launch'), + '/config/crop_box_filter_measurement_range.param.yaml' + ], + 'path to the parameter file of crop_box_filter_measurement_range' + ) + add_launch_arg( + 'voxel_grid_downsample_filter_param_path', + [ + FindPackageShare('localization_launch'), + '/config/voxel_grid_filter.param.yaml' + ], + 'path to the parameter file of voxel_grid_downsample_filter' + ) + add_launch_arg( + 'random_downsample_filter_param_path', + [ + FindPackageShare('localization_launch'), + '/config/random_downsample_filter.param.yaml' + ], + 'path to the parameter file of random_downsample_filter' + ) + add_launch_arg('use_intra_process', 'true', 'use ROS2 component container communication') + add_launch_arg( + 'container', + '/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container', + 'container name' + ) + add_launch_arg( + 'input_sensor_points_topic', + '/sensing/lidar/top/rectified/pointcloud', + 'input topic name for raw pointcloud' + ) + add_launch_arg( + 'output_measurement_range_sensor_points_topic', + 'measurement_range/pointcloud', + 'output topic name for crop box filter' + ) + add_launch_arg( + 'output_voxel_grid_downsample_sensor_points_topic', + 'voxel_grid_downsample/pointcloud', + 'output topic name for voxel grid downsample filter' + ) + add_launch_arg( + 'output_downsample_sensor_points_topic', + 'downsample/pointcloud', + 'output topic name for downsample filter. this is final output' + ) + + return launch.LaunchDescription( + launch_arguments + + [ + OpaqueFunction(function=launch_setup) + ] + ) diff --git a/localization_launch/launch/util/util.launch.xml b/localization_launch/launch/util/util.launch.xml index 5ea9bfce95b05..6d2cc29549aa6 100644 --- a/localization_launch/launch/util/util.launch.xml +++ b/localization_launch/launch/util/util.launch.xml @@ -7,9 +7,6 @@ - - -