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