diff --git a/sensing_launch/CMakeLists.txt b/sensing_launch/CMakeLists.txt index 036e5b89af4e3..408ba2fe4a7ec 100644 --- a/sensing_launch/CMakeLists.txt +++ b/sensing_launch/CMakeLists.txt @@ -11,5 +11,4 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch - data ) diff --git a/sensing_launch/data/traffic_light_camera.yaml b/sensing_launch/data/traffic_light_camera.yaml deleted file mode 100644 index 458ad17ca2e19..0000000000000 --- a/sensing_launch/data/traffic_light_camera.yaml +++ /dev/null @@ -1,20 +0,0 @@ -image_width: 1920 -image_height: 1080 -camera_name: traffic_light/camera -camera_matrix: - rows: 3 - cols: 3 - data: [2410.755261, 0.000000, 922.621401, 0.000000, 2403.573140, 534.752500, 0.000000, 0.000000, 1.000000] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.126600, 0.152594, 0.002432, -0.001244, 0.000000] -rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] -projection_matrix: - rows: 3 - cols: 4 - data: [2370.254883, 0.000000, 920.136018, 0.000000, 0.000000, 2388.885254, 535.599668, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/sensing_launch/launch/aip_customized/camera.launch.xml b/sensing_launch/launch/aip_customized/camera.launch.xml deleted file mode 100644 index 617d5fbb686f9..0000000000000 --- a/sensing_launch/launch/aip_customized/camera.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_customized/gnss.launch.xml b/sensing_launch/launch/aip_customized/gnss.launch.xml deleted file mode 100644 index 5f2b1a29e381b..0000000000000 --- a/sensing_launch/launch/aip_customized/gnss.launch.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_customized/imu.launch.xml b/sensing_launch/launch/aip_customized/imu.launch.xml deleted file mode 100644 index c7305b6924701..0000000000000 --- a/sensing_launch/launch/aip_customized/imu.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_customized/lidar.launch.xml b/sensing_launch/launch/aip_customized/lidar.launch.xml deleted file mode 100644 index 93d040ce3b65c..0000000000000 --- a/sensing_launch/launch/aip_customized/lidar.launch.xml +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py deleted file mode 100644 index a9e3b03e559d5..0000000000000 --- a/sensing_launch/launch/aip_customized/pointcloud_preprocessor.launch.py +++ /dev/null @@ -1,193 +0,0 @@ - -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -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 -import yaml - - -def get_vehicle_info(context): - path = LaunchConfiguration('vehicle_param_file').perform(context) - with open(path, 'r') as f: - p = yaml.safe_load(f)['/**']['ros__parameters'] - p['vehicle_length'] = p['front_overhang'] + p['wheel_base'] + p['rear_overhang'] - p['vehicle_width'] = p['wheel_tread'] + p['left_overhang'] + p['right_overhang'] - p['min_longitudinal_offset'] = -p['rear_overhang'] - p['max_longitudinal_offset'] = p['front_overhang'] + p['wheel_base'] - p['min_lateral_offset'] = -(p['wheel_tread'] / 2.0 + p['right_overhang']) - p['max_lateral_offset'] = p['wheel_tread'] / 2.0 + p['left_overhang'] - p['min_height_offset'] = 0.0 - p['max_height_offset'] = p['vehicle_height'] - 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) - return p - - -def launch_setup(context, *args, **kwargs): - - pkg = 'pointcloud_preprocessor' - - vehicle_info = get_vehicle_info(context) - - # set concat filter as a component - concat_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent', - name='concatenate_data', - remappings=[('output', 'concatenated/pointcloud')], - parameters=[{ - 'input_topics': ['/sensing/lidar/top/outlier_filtered/pointcloud', - '/sensing/lidar/left/outlier_filtered/pointcloud', - '/sensing/lidar/right/outlier_filtered/pointcloud'], - 'output_frame': LaunchConfiguration('base_frame'), - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - passthrough_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PassThroughFilterComponent', - name='passthrough_filter', - remappings=[ - ('input', 'top/outlier_filtered/pointcloud'), - ('output', 'concatenated/pointcloud'), - ], - parameters=[{ - 'output_frame': LaunchConfiguration('base_frame'), - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set crop box filter as a component - cropbox_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='crop_box_filter', - remappings=[ - ('input', 'concatenated/pointcloud'), - ('output', 'measurement_range_cropped/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': -50.0, - 'max_x': 100.0, - 'min_y': -50.0, - 'max_y': 50.0, - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - 'negative': False, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - ground_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::RayGroundFilterComponent', - name='ray_ground_filter', - remappings=[ - ('input', 'measurement_range_cropped/pointcloud'), - ('output', 'no_ground/pointcloud') - ], - parameters=[{ - 'general_max_slope': 10.0, - 'local_max_slope': 10.0, - 'min_height_threshold': 0.2, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name='pointcloud_preprocessor_container', - namespace='pointcloud_preprocessor', - package='rclcpp_components', - executable=LaunchConfiguration('container_executable'), - composable_node_descriptions=[ - cropbox_component, - ground_component, - ], - output='screen', - ) - - # load concat or passthrough filter - concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component], - target_container=container, - condition=IfCondition(LaunchConfiguration('use_concat_filter')), - ) - - passthrough_loader = LoadComposableNodes( - composable_node_descriptions=[passthrough_component], - target_container=container, - condition=UnlessCondition(LaunchConfiguration('use_concat_filter')), - ) - - return [container, concat_loader, passthrough_loader] - - -def generate_launch_description(): - - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg('base_frame', 'base_link') - add_launch_arg('use_concat_filter', 'use_concat_filter') - add_launch_arg('vehicle_param_file') - add_launch_arg('use_multithread', 'False') - add_launch_arg('use_intra_process', 'False') - - set_container_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container', - condition=UnlessCondition(LaunchConfiguration('use_multithread')) - ) - - set_container_mt_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container_mt', - condition=IfCondition(LaunchConfiguration('use_multithread')) - ) - - return launch.LaunchDescription(launch_arguments + - [set_container_executable, - set_container_mt_executable] + - [OpaqueFunction(function=launch_setup)]) diff --git a/sensing_launch/launch/aip_s1/camera.launch.xml b/sensing_launch/launch/aip_s1/camera.launch.xml deleted file mode 100644 index 617d5fbb686f9..0000000000000 --- a/sensing_launch/launch/aip_s1/camera.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_s1/gnss.launch.xml b/sensing_launch/launch/aip_s1/gnss.launch.xml deleted file mode 100644 index 5f2b1a29e381b..0000000000000 --- a/sensing_launch/launch/aip_s1/gnss.launch.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_s1/imu.launch.xml b/sensing_launch/launch/aip_s1/imu.launch.xml deleted file mode 100644 index e32d7dc97c35d..0000000000000 --- a/sensing_launch/launch/aip_s1/imu.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_s1/lidar.launch.xml b/sensing_launch/launch/aip_s1/lidar.launch.xml deleted file mode 100644 index f87e7d153827a..0000000000000 --- a/sensing_launch/launch/aip_s1/lidar.launch.xml +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_s1/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_s1/pointcloud_preprocessor.launch.py deleted file mode 100644 index 438fee810be0b..0000000000000 --- a/sensing_launch/launch/aip_s1/pointcloud_preprocessor.launch.py +++ /dev/null @@ -1,191 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -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 -import yaml - - -def get_vehicle_info(context): - path = LaunchConfiguration('vehicle_param_file').perform(context) - with open(path, 'r') as f: - p = yaml.safe_load(f)['/**']['ros__parameters'] - p['vehicle_length'] = p['front_overhang'] + p['wheel_base'] + p['rear_overhang'] - p['vehicle_width'] = p['wheel_tread'] + p['left_overhang'] + p['right_overhang'] - p['min_longitudinal_offset'] = -p['rear_overhang'] - p['max_longitudinal_offset'] = p['front_overhang'] + p['wheel_base'] - p['min_lateral_offset'] = -(p['wheel_tread'] / 2.0 + p['right_overhang']) - p['max_lateral_offset'] = p['wheel_tread'] / 2.0 + p['left_overhang'] - p['min_height_offset'] = 0.0 - p['max_height_offset'] = p['vehicle_height'] - 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) - return p - - -def launch_setup(context, *args, **kwargs): - - pkg = 'pointcloud_preprocessor' - - vehicle_info = get_vehicle_info(context) - - # set concat filter as a component - concat_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent', - name='concatenate_data', - remappings=[('output', 'concatenated/pointcloud')], - parameters=[{ - 'input_topics': ['/sensing/lidar/top/outlier_filtered/pointcloud', - '/sensing/lidar/left/outlier_filtered/pointcloud', - '/sensing/lidar/right/outlier_filtered/pointcloud'], - 'output_frame': LaunchConfiguration('base_frame'), - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - passthrough_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PassThroughFilterComponent', - name='passthrough_filter', - remappings=[ - ('input', 'top/outlier_filtered/pointcloud'), - ('output', 'concatenated/pointcloud'), - ], - parameters=[{ - 'output_frame': LaunchConfiguration('base_frame'), - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set crop box filter as a component - cropbox_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='crop_box_filter', - remappings=[ - ('input', 'concatenated/pointcloud'), - ('output', 'measurement_range_cropped/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': -50.0, - 'max_x': 100.0, - 'min_y': -50.0, - 'max_y': 50.0, - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - 'negative': False, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - ground_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::RayGroundFilterComponent', - name='ray_ground_filter', - remappings=[ - ('input', 'measurement_range_cropped/pointcloud'), - ('output', 'no_ground/pointcloud') - ], - parameters=[{ - 'general_max_slope': 10.0, - 'local_max_slope': 10.0, - 'min_height_threshold': 0.2, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name='pointcloud_preprocessor_container', - namespace='pointcloud_preprocessor', - package='rclcpp_components', - executable=LaunchConfiguration('container_executable'), - composable_node_descriptions=[ - cropbox_component, - ground_component, - ], - output='screen', - ) - - # load concat or passthrough filter - concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component], - target_container=container, - condition=IfCondition(LaunchConfiguration('use_concat_filter')), - ) - - passthrough_loader = LoadComposableNodes( - composable_node_descriptions=[passthrough_component], - target_container=container, - condition=UnlessCondition(LaunchConfiguration('use_concat_filter')), - ) - - return [container, concat_loader, passthrough_loader] - - -def generate_launch_description(): - - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg('base_frame', 'base_link') - add_launch_arg('use_concat_filter', 'use_concat_filter') - add_launch_arg('vehicle_param_file') - add_launch_arg('use_multithread', 'False') - add_launch_arg('use_intra_process', 'False') - - set_container_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container', - condition=UnlessCondition(LaunchConfiguration('use_multithread')) - ) - - set_container_mt_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container_mt', - condition=IfCondition(LaunchConfiguration('use_multithread')) - ) - - return launch.LaunchDescription(launch_arguments + - [set_container_executable, - set_container_mt_executable] + - [OpaqueFunction(function=launch_setup)]) diff --git a/sensing_launch/launch/aip_x1/camera.launch.xml b/sensing_launch/launch/aip_x1/camera.launch.xml deleted file mode 100644 index 525b5285fc67b..0000000000000 --- a/sensing_launch/launch/aip_x1/camera.launch.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_x1/gnss.launch.xml b/sensing_launch/launch/aip_x1/gnss.launch.xml deleted file mode 100644 index a388e05f43853..0000000000000 --- a/sensing_launch/launch/aip_x1/gnss.launch.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_x1/imu.launch.xml b/sensing_launch/launch/aip_x1/imu.launch.xml deleted file mode 100644 index 0771709bda43b..0000000000000 --- a/sensing_launch/launch/aip_x1/imu.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_x1/lidar.launch.xml b/sensing_launch/launch/aip_x1/lidar.launch.xml deleted file mode 100644 index fa1212a7aae56..0000000000000 --- a/sensing_launch/launch/aip_x1/lidar.launch.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_x1/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_x1/pointcloud_preprocessor.launch.py deleted file mode 100644 index e577f912ae0c9..0000000000000 --- a/sensing_launch/launch/aip_x1/pointcloud_preprocessor.launch.py +++ /dev/null @@ -1,378 +0,0 @@ - -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -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 -import yaml - - -def get_vehicle_info(context): - path = LaunchConfiguration('vehicle_param_file').perform(context) - with open(path, 'r') as f: - p = yaml.safe_load(f)['/**']['ros__parameters'] - p['vehicle_length'] = p['front_overhang'] + p['wheel_base'] + p['rear_overhang'] - p['vehicle_width'] = p['wheel_tread'] + p['left_overhang'] + p['right_overhang'] - p['min_longitudinal_offset'] = -p['rear_overhang'] - p['max_longitudinal_offset'] = p['front_overhang'] + p['wheel_base'] - p['min_lateral_offset'] = -(p['wheel_tread'] / 2.0 + p['right_overhang']) - p['max_lateral_offset'] = p['wheel_tread'] / 2.0 + p['left_overhang'] - p['min_height_offset'] = 0.0 - p['max_height_offset'] = p['vehicle_height'] - 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): - - pkg = 'pointcloud_preprocessor' - - vehicle_info = get_vehicle_info(context) - - # set concat filter as a component - concat_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent', - name='concatenate_data', - remappings=[('output', 'concatenated/pointcloud')], - parameters=[{ - 'input_topics': ['/sensing/lidar/top/outlier_filtered/pointcloud', - '/sensing/lidar/front_left/mirror_cropped/pointcloud', - '/sensing/lidar/front_right/mirror_cropped/pointcloud', - '/sensing/lidar/front_center/mirror_cropped/pointcloud'], - 'output_frame': LaunchConfiguration('base_frame'), - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set PointCloud PassThrough Filter as a component - passthrough_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PassThroughFilterComponent', - name='passthrough_filter', - remappings=[ - ('input', 'top/outlier_filtered/pointcloud'), - ('output', 'concatenated/pointcloud'), - ], - parameters=[{ - 'output_frame': LaunchConfiguration('base_frame'), - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set crop box filter as a component - cropbox_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='crop_box_filter', - remappings=[ - ('input', 'concatenated/pointcloud'), - ('output', 'measurement_range_cropped/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': -50.0, - 'max_x': 100.0, - 'min_y': -50.0, - 'max_y': 50.0, - 'min_z': -0.5, - 'max_z': vehicle_info['max_height_offset'], - 'negative': False, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - ray_ground_filter_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::RayGroundFilterComponent', - name='ray_ground_filter', - remappings=[ - ('input', 'measurement_range_cropped/pointcloud'), - ('output', 'rough/no_ground/pointcloud'), - ], - parameters=[{ - 'initial_max_slope': 10.0, - 'general_max_slope': 10.0, - 'local_max_slope': 10.0, - 'min_height_threshold': 0.3, - 'radial_divider_angle': 1.0, - 'concentric_divider_distance': 0.0, - 'use_vehicle_footprint': True, - 'min_x': vehicle_info['min_longitudinal_offset'], - 'max_x': vehicle_info['max_longitudinal_offset'], - 'min_y': vehicle_info['min_lateral_offset'], - 'max_y': vehicle_info['max_lateral_offset'], - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - short_height_obstacle_detection_area_filter_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='short_height_obstacle_detection_area_filter', - remappings=[ - ('input', 'front_center/mirror_cropped/pointcloud'), - ('output', 'short_height_obstacle_detection_area/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': 0.0, - 'max_x': 15.6, # max_x: 14.0m + base_link2livox_front_center distance 1.6m - 'min_y': -4.0, - 'max_y': 4.0, - 'min_z': -0.5, - 'max_z': 0.5, - 'negative': False, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - vector_map_filter_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::Lanelet2MapFilterComponent', - name='vector_map_filter', - remappings=[ - ('input/pointcloud', 'short_height_obstacle_detection_area/pointcloud'), - ('input/vector_map', '/map/vector_map'), - ('output', 'vector_map_filtered/pointcloud'), - ], - parameters=[{ - 'voxel_size_x': 0.25, - 'voxel_size_y': 0.25, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - ransac_ground_filter_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::RANSACGroundFilterComponent', - name='ransac_ground_filter', - remappings=[ - ('input', 'vector_map_filtered/pointcloud'), - ('output', 'short_height/no_ground/pointcloud'), - ], - parameters=[{ - 'outlier_threshold': 0.1, - 'min_points': 400, - 'min_inliers': 200, - 'max_iterations': 50, - 'height_threshold': 0.12, - 'plane_slope_threshold': 10.0, - 'voxel_size_x': 0.2, - 'voxel_size_y': 0.2, - 'voxel_size_z': 0.2, - 'debug': False, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - concat_no_ground_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent', - name='concatenate_no_ground_data', - remappings=[('output', 'no_ground/concatenated/pointcloud')], - parameters=[{ - 'input_topics': ['/sensing/lidar/rough/no_ground/pointcloud', - '/sensing/lidar/short_height/no_ground/pointcloud'], - 'output_frame': LaunchConfiguration('base_frame'), - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - voxel_grid_filter_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::VoxelGridDownsampleFilterComponent', - name='voxel_grid_filter', - remappings=[ - ('input', 'no_ground/concatenated/pointcloud'), - ('output', 'voxel_grid_filtered/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'voxel_size_x': 0.04, - 'voxel_size_y': 0.04, - 'voxel_size_z': 0.08, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - radius_search_2d_outlier_filter_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::RadiusSearch2dOutlierFilterComponent', - name='radius_search_2d_outlier_filter', - remappings=[ - ('input', 'voxel_grid_filtered/pointcloud'), - ('output', 'no_ground/pointcloud'), - ], - parameters=[{ - 'search_radius': 0.2, - 'min_neighbors': 5, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - voxel_grid_outlier_filter_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::VoxelGridOutlierFilterComponent', - name='voxel_grid_outlier_filter', - remappings=[ - ('input', 'voxel_grid_filtered/pointcloud'), - ('output', 'no_ground/pointcloud'), - ], - parameters=[{ - 'voxel_size_x': 0.4, - 'voxel_size_y': 0.4, - 'voxel_size_z': 100.0, - 'voxel_points_threshold': 5, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - relay_component = ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='relay', - parameters=[{ - 'input_topic': '/sensing/lidar/top/outlier_filtered/pointcloud', - 'output_topic': '/sensing/lidar/pointcloud', - 'type': 'sensor_msgs/msg/PointCloud2', - 'history': 'keep_last', - 'depth': 5, - 'reliability': 'best_effort', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name='pointcloud_preprocessor_container', - namespace='pointcloud_preprocessor', - package='rclcpp_components', - executable=LaunchConfiguration('container_executable'), - composable_node_descriptions=[ - cropbox_component, - ray_ground_filter_component, - short_height_obstacle_detection_area_filter_component, - vector_map_filter_component, - ransac_ground_filter_component, - concat_no_ground_component, - voxel_grid_filter_component, - relay_component, - ], - output='screen', - ) - - # load concat or passthrough filter - concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration('use_concat_filter')), - ) - - passthrough_loader = LoadComposableNodes( - composable_node_descriptions=[passthrough_component], - target_container=container, - condition=launch.conditions.UnlessCondition(LaunchConfiguration('use_concat_filter')), - ) - - # load concat or passthrough filter - radius_search_2d_outlier_filter_loader = LoadComposableNodes( - composable_node_descriptions=[radius_search_2d_outlier_filter_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration('use_radius_search')), - ) - - voxel_grid_outlier_filter_loader = LoadComposableNodes( - composable_node_descriptions=[voxel_grid_outlier_filter_component], - target_container=container, - condition=launch.conditions.UnlessCondition(LaunchConfiguration('use_radius_search')), - ) - return [container, concat_loader, passthrough_loader, - radius_search_2d_outlier_filter_loader, - voxel_grid_outlier_filter_loader] - - -def generate_launch_description(): - - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg('base_frame', 'base_link') - add_launch_arg('use_concat_filter', 'true') - add_launch_arg('use_radius_search', 'true') - add_launch_arg('vehicle_param_file') - add_launch_arg('use_multithread', 'False') - add_launch_arg('use_intra_process', 'False') - - set_container_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container', - condition=UnlessCondition(LaunchConfiguration('use_multithread')) - ) - - set_container_mt_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container_mt', - condition=IfCondition(LaunchConfiguration('use_multithread')) - ) - - return launch.LaunchDescription(launch_arguments + - [set_container_executable, - set_container_mt_executable] + - [OpaqueFunction(function=launch_setup)]) diff --git a/sensing_launch/launch/aip_x2/camera.launch.xml b/sensing_launch/launch/aip_x2/camera.launch.xml deleted file mode 100644 index 617d5fbb686f9..0000000000000 --- a/sensing_launch/launch/aip_x2/camera.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_x2/gnss.launch.xml b/sensing_launch/launch/aip_x2/gnss.launch.xml deleted file mode 100644 index 5f2b1a29e381b..0000000000000 --- a/sensing_launch/launch/aip_x2/gnss.launch.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_x2/imu.launch.xml b/sensing_launch/launch/aip_x2/imu.launch.xml deleted file mode 100644 index c22b05172abfb..0000000000000 --- a/sensing_launch/launch/aip_x2/imu.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_x2/lidar.launch.xml b/sensing_launch/launch/aip_x2/lidar.launch.xml deleted file mode 100644 index ecaf554a04882..0000000000000 --- a/sensing_launch/launch/aip_x2/lidar.launch.xml +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_x2/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_x2/pointcloud_preprocessor.launch.py deleted file mode 100644 index 2da76720bf122..0000000000000 --- a/sensing_launch/launch/aip_x2/pointcloud_preprocessor.launch.py +++ /dev/null @@ -1,182 +0,0 @@ - -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -import yaml - - -def get_vehicle_info(context): - path = LaunchConfiguration('vehicle_param_file').perform(context) - with open(path, 'r') as f: - p = yaml.safe_load(f)['/**']['ros__parameters'] - p['vehicle_length'] = p['front_overhang'] + p['wheel_base'] + p['rear_overhang'] - p['vehicle_width'] = p['wheel_tread'] + p['left_overhang'] + p['right_overhang'] - p['min_longitudinal_offset'] = -p['rear_overhang'] - p['max_longitudinal_offset'] = p['front_overhang'] + p['wheel_base'] - p['min_lateral_offset'] = -(p['wheel_tread'] / 2.0 + p['right_overhang']) - p['max_lateral_offset'] = p['wheel_tread'] / 2.0 + p['left_overhang'] - p['min_height_offset'] = 0.0 - p['max_height_offset'] = p['rear_overhang'] - 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) - return p - - -def launch_setup(context, *args, **kwargs): - - pkg = 'pointcloud_preprocessor' - - vehicle_info = get_vehicle_info(context) - - # set concat filter as a component - if (LaunchConfiguration('use_concat_filter')): - concat_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent', - name='concatenate_data', - remappings=[('/output', 'concatenated/pointcloud')], - parameters=[{ - 'input_topics': ['/sensing/lidar/top/outlier_filtered/pointcloud', - '/sensing/lidar/left/outlier_filtered/pointcloud', - '/sensing/lidar/right/outlier_filtered/pointcloud'], - 'output_frame': 'base_link', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - else: - # set PointCloud PassThrough Filter as a component - concat_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PassThroughFilterComponent', - name='passthrough_filter', - remappings=[ - ('/input', 'top/outlier_filtered/pointcloud'), - ('/output', 'concatenated/pointcloud'), - ], - parameters=[{ - 'output_frame': 'base_link', - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set crop box filter as a component - cropbox_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='crop_box_filter', - remappings=[ - ('/input', 'concatenated/pointcloud'), - ('/output', 'measurement_range_cropped/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': -50.0, - 'max_x': 100.0, - 'min_y': -50.0, - 'max_y': 50.0, - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - 'negative': False, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - ground_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::RayGroundFilterComponent', - name='ray_ground_filter', - remappings=[ - ('/input', 'measurement_range_cropped/pointcloud'), - ('/output', 'no_ground/pointcloud') - ], - parameters=[{ - 'general_max_slope': 10.0, - 'local_max_slope': 10.0, - 'min_height_threshold': 0.2, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name='pointcloud_preprocessor_container', - namespace='pointcloud_preprocessor', - package='rclcpp_components', - executable=LaunchConfiguration('container_executable'), - composable_node_descriptions=[ - concat_component, - cropbox_component, - ground_component, - ], - output='screen', - ) - - return [container] - - -def generate_launch_description(): - - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg('base_frame', 'base_link') - add_launch_arg('use_concat_filter', 'use_concat_filter') - add_launch_arg('vehicle_param_file') - add_launch_arg('use_multithread', 'False') - add_launch_arg('use_intra_process', 'False') - - set_container_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container', - condition=UnlessCondition(LaunchConfiguration('use_multithread')) - ) - - set_container_mt_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container_mt', - condition=IfCondition(LaunchConfiguration('use_multithread')) - ) - - return launch.LaunchDescription(launch_arguments + - [set_container_executable, - set_container_mt_executable] + - [OpaqueFunction(function=launch_setup)]) diff --git a/sensing_launch/launch/aip_xx1/camera.launch.xml b/sensing_launch/launch/aip_xx1/camera.launch.xml deleted file mode 100644 index 09cfea332e3c0..0000000000000 --- a/sensing_launch/launch/aip_xx1/camera.launch.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_xx1/gnss.launch.xml b/sensing_launch/launch/aip_xx1/gnss.launch.xml deleted file mode 100644 index 5370dbeb65eff..0000000000000 --- a/sensing_launch/launch/aip_xx1/gnss.launch.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_xx1/imu.launch.xml b/sensing_launch/launch/aip_xx1/imu.launch.xml deleted file mode 100644 index 705756e703fef..0000000000000 --- a/sensing_launch/launch/aip_xx1/imu.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_xx1/lidar.launch.xml b/sensing_launch/launch/aip_xx1/lidar.launch.xml deleted file mode 100644 index 2300103af51c7..0000000000000 --- a/sensing_launch/launch/aip_xx1/lidar.launch.xml +++ /dev/null @@ -1,103 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py deleted file mode 100644 index a9e3b03e559d5..0000000000000 --- a/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py +++ /dev/null @@ -1,193 +0,0 @@ - -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -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 -import yaml - - -def get_vehicle_info(context): - path = LaunchConfiguration('vehicle_param_file').perform(context) - with open(path, 'r') as f: - p = yaml.safe_load(f)['/**']['ros__parameters'] - p['vehicle_length'] = p['front_overhang'] + p['wheel_base'] + p['rear_overhang'] - p['vehicle_width'] = p['wheel_tread'] + p['left_overhang'] + p['right_overhang'] - p['min_longitudinal_offset'] = -p['rear_overhang'] - p['max_longitudinal_offset'] = p['front_overhang'] + p['wheel_base'] - p['min_lateral_offset'] = -(p['wheel_tread'] / 2.0 + p['right_overhang']) - p['max_lateral_offset'] = p['wheel_tread'] / 2.0 + p['left_overhang'] - p['min_height_offset'] = 0.0 - p['max_height_offset'] = p['vehicle_height'] - 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) - return p - - -def launch_setup(context, *args, **kwargs): - - pkg = 'pointcloud_preprocessor' - - vehicle_info = get_vehicle_info(context) - - # set concat filter as a component - concat_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent', - name='concatenate_data', - remappings=[('output', 'concatenated/pointcloud')], - parameters=[{ - 'input_topics': ['/sensing/lidar/top/outlier_filtered/pointcloud', - '/sensing/lidar/left/outlier_filtered/pointcloud', - '/sensing/lidar/right/outlier_filtered/pointcloud'], - 'output_frame': LaunchConfiguration('base_frame'), - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - passthrough_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::PassThroughFilterComponent', - name='passthrough_filter', - remappings=[ - ('input', 'top/outlier_filtered/pointcloud'), - ('output', 'concatenated/pointcloud'), - ], - parameters=[{ - 'output_frame': LaunchConfiguration('base_frame'), - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set crop box filter as a component - cropbox_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='crop_box_filter', - remappings=[ - ('input', 'concatenated/pointcloud'), - ('output', 'measurement_range_cropped/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': -50.0, - 'max_x': 100.0, - 'min_y': -50.0, - 'max_y': 50.0, - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - 'negative': False, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - ground_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::RayGroundFilterComponent', - name='ray_ground_filter', - remappings=[ - ('input', 'measurement_range_cropped/pointcloud'), - ('output', 'no_ground/pointcloud') - ], - parameters=[{ - 'general_max_slope': 10.0, - 'local_max_slope': 10.0, - 'min_height_threshold': 0.2, - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name='pointcloud_preprocessor_container', - namespace='pointcloud_preprocessor', - package='rclcpp_components', - executable=LaunchConfiguration('container_executable'), - composable_node_descriptions=[ - cropbox_component, - ground_component, - ], - output='screen', - ) - - # load concat or passthrough filter - concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component], - target_container=container, - condition=IfCondition(LaunchConfiguration('use_concat_filter')), - ) - - passthrough_loader = LoadComposableNodes( - composable_node_descriptions=[passthrough_component], - target_container=container, - condition=UnlessCondition(LaunchConfiguration('use_concat_filter')), - ) - - return [container, concat_loader, passthrough_loader] - - -def generate_launch_description(): - - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg('base_frame', 'base_link') - add_launch_arg('use_concat_filter', 'use_concat_filter') - add_launch_arg('vehicle_param_file') - add_launch_arg('use_multithread', 'False') - add_launch_arg('use_intra_process', 'False') - - set_container_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container', - condition=UnlessCondition(LaunchConfiguration('use_multithread')) - ) - - set_container_mt_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container_mt', - condition=IfCondition(LaunchConfiguration('use_multithread')) - ) - - return launch.LaunchDescription(launch_arguments + - [set_container_executable, - set_container_mt_executable] + - [OpaqueFunction(function=launch_setup)]) diff --git a/sensing_launch/launch/livox_horizon.launch.py b/sensing_launch/launch/livox_horizon.launch.py deleted file mode 100644 index af5a5560eef18..0000000000000 --- a/sensing_launch/launch/livox_horizon.launch.py +++ /dev/null @@ -1,170 +0,0 @@ - -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode -import yaml - - -def get_vehicle_info(context): - path = LaunchConfiguration('vehicle_param_file').perform(context) - with open(path, 'r') as f: - p = yaml.safe_load(f)['/**']['ros__parameters'] - p['vehicle_length'] = p['front_overhang'] + p['wheel_base'] + p['rear_overhang'] - p['vehicle_width'] = p['wheel_tread'] + p['left_overhang'] + p['right_overhang'] - p['min_longitudinal_offset'] = -p['rear_overhang'] - p['max_longitudinal_offset'] = p['front_overhang'] + p['wheel_base'] - p['min_lateral_offset'] = -(p['wheel_tread'] / 2.0 + p['right_overhang']) - p['max_lateral_offset'] = p['wheel_tread'] / 2.0 + p['left_overhang'] - p['min_height_offset'] = 0.0 - p['max_height_offset'] = p['vehicle_height'] - 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): - - pkg = 'pointcloud_preprocessor' - - vehicle_info = get_vehicle_info(context) - vehicle_mirror_info = get_vehicle_mirror_info(context) - - bd_code_param_path = LaunchConfiguration('bd_code_param_path').perform(context) - with open(bd_code_param_path, 'r') as f: - bd_code_param = yaml.safe_load(f)['/**']['ros__parameters'] - - # livox driver - livox_driver_component = ComposableNode( - package='livox_ros2_driver', - plugin='livox_ros::LivoxDriver', - name='livox_driver', - parameters=[ - { - 'xfe_format': LaunchConfiguration('xfe_format'), - 'multi_topic': LaunchConfiguration('multi_topic'), - 'data_src': LaunchConfiguration('data_src'), - 'publish_freq': LaunchConfiguration('publish_freq'), - 'output_data_type': LaunchConfiguration('output_type'), - 'lvx_file_path': LaunchConfiguration('lvx_file_path'), - 'user_config_path': LaunchConfiguration('user_config_path'), - 'frame_id': LaunchConfiguration('sensor_frame'), - }, - bd_code_param, - ] - ) - - # set self crop box filter as a component - cropbox_self_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='self_crop_box_filter', - remappings=[ - ('input', 'livox/lidar'), - ('output', 'self_cropped/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': vehicle_info['min_longitudinal_offset'], - 'max_x': vehicle_info['max_longitudinal_offset'], - 'min_y': vehicle_info['min_lateral_offset'], - 'max_y': vehicle_info['max_lateral_offset'], - 'min_z': vehicle_info['min_height_offset'], - 'max_z': vehicle_info['max_height_offset'], - 'negative': True, - }] - ) - - # set mirror crop box filter as a component - cropbox_mirror_component = ComposableNode( - package=pkg, - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='mirror_crop_box_filter', - remappings=[ - ('input', 'self_cropped/pointcloud'), - ('output', 'mirror_cropped/pointcloud'), - ], - parameters=[{ - 'input_frame': LaunchConfiguration('base_frame'), - 'output_frame': LaunchConfiguration('base_frame'), - 'min_x': vehicle_mirror_info['min_longitudinal_offset'], - 'max_x': vehicle_mirror_info['max_longitudinal_offset'], - 'min_y': vehicle_mirror_info['min_lateral_offset'], - 'max_y': vehicle_mirror_info['max_lateral_offset'], - 'min_z': vehicle_mirror_info['min_height_offset'], - 'max_z': vehicle_mirror_info['max_height_offset'], - 'negative': True, - }] - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name='pointcloud_preprocessor_container', - namespace='pointcloud_preprocessor', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - cropbox_self_component, - cropbox_mirror_component, - ], - output='screen', - ) - - loader = LoadComposableNodes( - composable_node_descriptions=[livox_driver_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration('launch_driver')), - ) - - return [container, loader] - - -def generate_launch_description(): - - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg('xfe_format', '0') - add_launch_arg('multi_topic', '0') - add_launch_arg('data_src', '0') - add_launch_arg('publish_freq', '10.0') - add_launch_arg('output_type', '0') - add_launch_arg('lvx_file_path', 'livox_test.lvx') - add_launch_arg('user_config_path', os.path.join(get_package_share_directory( - 'livox_ros2_driver'), 'config/livox_lidar_config.json')) - add_launch_arg('bd_code_param_path') - add_launch_arg('launch_driver') - add_launch_arg('base_frame', 'base_link') - add_launch_arg('sensor_frame', 'livox_frame') - add_launch_arg('vehicle_param_file') - add_launch_arg('vehicle_mirror_param_file') - - return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/sensing_launch/launch/sensing.launch.xml b/sensing_launch/launch/sensing.launch.xml index 91bc779575305..8795835878856 100644 --- a/sensing_launch/launch/sensing.launch.xml +++ b/sensing_launch/launch/sensing.launch.xml @@ -5,31 +5,18 @@ + + - - + + - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/velodyne_VLP16.launch.xml b/sensing_launch/launch/velodyne_VLP16.launch.xml deleted file mode 100644 index 61b60884b52a8..0000000000000 --- a/sensing_launch/launch/velodyne_VLP16.launch.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/velodyne_VLP32C.launch.xml b/sensing_launch/launch/velodyne_VLP32C.launch.xml deleted file mode 100644 index 208f9da8ad60c..0000000000000 --- a/sensing_launch/launch/velodyne_VLP32C.launch.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/velodyne_VLS128.launch.xml b/sensing_launch/launch/velodyne_VLS128.launch.xml deleted file mode 100644 index 0820e641b9d19..0000000000000 --- a/sensing_launch/launch/velodyne_VLS128.launch.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing_launch/launch/velodyne_node_container.launch.py b/sensing_launch/launch/velodyne_node_container.launch.py deleted file mode 100644 index ae3af1ad000a4..0000000000000 --- a/sensing_launch/launch/velodyne_node_container.launch.py +++ /dev/null @@ -1,242 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -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 -import yaml - - -def get_vehicle_info(context): - path = LaunchConfiguration('vehicle_param_file').perform(context) - with open(path, 'r') as f: - p = yaml.safe_load(f)['/**']['ros__parameters'] - p['vehicle_length'] = p['front_overhang'] + p['wheel_base'] + p['rear_overhang'] - p['vehicle_width'] = p['wheel_tread'] + p['left_overhang'] + p['right_overhang'] - p['min_longitudinal_offset'] = -p['rear_overhang'] - p['max_longitudinal_offset'] = p['front_overhang'] + p['wheel_base'] - p['min_lateral_offset'] = -(p['wheel_tread'] / 2.0 + p['right_overhang']) - p['max_lateral_offset'] = p['wheel_tread'] / 2.0 + p['left_overhang'] - p['min_height_offset'] = 0.0 - p['max_height_offset'] = p['vehicle_height'] - 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 create_parameter_dict(*args): - result = {} - for x in args: - result[x] = LaunchConfiguration(x) - return result - - nodes = [] - - # turn packets into pointcloud as in - # https://github.com/ros-drivers/velodyne/blob/ros2/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-composed-launch.py - nodes.append(ComposableNode( - package='velodyne_pointcloud', - plugin='velodyne_pointcloud::Convert', - name='velodyne_convert_node', - parameters=[{**create_parameter_dict('calibration', 'min_range', 'max_range', - 'num_points_thresholds', - 'invalid_intensity', - 'frame_id', 'scan_phase', - 'view_direction', 'view_width'), - }], - remappings=[('velodyne_points', 'pointcloud_raw'), - ('velodyne_points_ex', 'pointcloud_raw_ex')], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - ) - - cropbox_parameters = create_parameter_dict('input_frame', 'output_frame') - cropbox_parameters['negative'] = True - - vehicle_info = get_vehicle_info(context) - cropbox_parameters['min_x'] = vehicle_info['min_longitudinal_offset'] - cropbox_parameters['max_x'] = vehicle_info['max_longitudinal_offset'] - cropbox_parameters['min_y'] = vehicle_info['min_lateral_offset'] - cropbox_parameters['max_y'] = vehicle_info['max_lateral_offset'] - cropbox_parameters['min_z'] = vehicle_info['min_height_offset'] - cropbox_parameters['max_z'] = vehicle_info['max_height_offset'] - - nodes.append(ComposableNode( - package='pointcloud_preprocessor', - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='crop_box_filter_self', - remappings=[('input', 'pointcloud_raw_ex'), - ('output', 'self_cropped/pointcloud_ex'), - ], - parameters=[cropbox_parameters], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - ) - - mirror_info = get_vehicle_mirror_info(context) - 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'] - cropbox_parameters['max_y'] = mirror_info['max_lateral_offset'] - cropbox_parameters['min_z'] = mirror_info['min_height_offset'] - cropbox_parameters['max_z'] = mirror_info['max_height_offset'] - - nodes.append(ComposableNode( - package='pointcloud_preprocessor', - plugin='pointcloud_preprocessor::CropBoxFilterComponent', - name='crop_box_filter_mirror', - remappings=[('input', 'self_cropped/pointcloud_ex'), - ('output', 'mirror_cropped/pointcloud_ex'), - ], - parameters=[cropbox_parameters], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - ) - - nodes.append(ComposableNode( - package='velodyne_pointcloud', - plugin='velodyne_pointcloud::Interpolate', - name='velodyne_interpolate_node', - remappings=[ - ('velodyne_points_ex', 'mirror_cropped/pointcloud_ex'), - ('velodyne_points_interpolate', 'rectified/pointcloud'), - ('velodyne_points_interpolate_ex', 'rectified/pointcloud_ex'), - ], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - ) - - nodes.append(ComposableNode( - package='pointcloud_preprocessor', - plugin='pointcloud_preprocessor::RingOutlierFilterComponent', - name='ring_outlier_filter', - remappings=[ - ('input', 'rectified/pointcloud_ex'), - ('output', 'outlier_filtered/pointcloud') - ], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - ) - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - # need unique name, otherwise all processes in same container and the node names then clash - name='velodyne_node_container', - namespace='pointcloud_preprocessor', - package='rclcpp_components', - executable=LaunchConfiguration('container_executable'), - composable_node_descriptions=nodes, - ) - - driver_component = ComposableNode( - package='velodyne_driver', - plugin='velodyne_driver::VelodyneDriver', - # node is created in a global context, need to avoid name clash - name='velodyne_driver', - parameters=[{**create_parameter_dict('device_ip', 'gps_time', 'read_once', 'read_fast', - 'repeat_delay', 'frame_id', 'model', 'rpm', 'port', - 'pcap', 'scan_phase'), - }], - ) - - # one way to add a ComposableNode conditional on a launch argument to a - # container. The `ComposableNode` itself doesn't accept a condition - loader = LoadComposableNodes( - composable_node_descriptions=[driver_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration('launch_driver')), - ) - - return [container, loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - # a default_value of None is equivalent to not passing that kwarg at all - launch_arguments.append(DeclareLaunchArgument( - name, default_value=default_value, description=description)) - - add_launch_arg('model', description='velodyne model name') - add_launch_arg('launch_driver', 'True', 'do launch driver') - add_launch_arg('calibration', description='path to calibration file') - add_launch_arg('device_ip', '192.168.1.201', 'device ip address') - add_launch_arg('scan_phase', '0.0') - add_launch_arg('base_frame', 'base_link', 'base frame id') - add_launch_arg('container_name', 'velodyne_composable_node_container', 'container name') - add_launch_arg('min_range', description='minimum view range') - add_launch_arg('max_range', description='maximum view range') - add_launch_arg('pcap', '') - add_launch_arg('port', '2368', description='device port number') - add_launch_arg('read_fast', 'False') - add_launch_arg('read_once', 'False') - add_launch_arg('repeat_delay', '0.0') - add_launch_arg('rpm', '600.0', 'rotational frequency') - add_launch_arg('laserscan_ring', '-1') - add_launch_arg('laserscan_resolution', '0.007') - add_launch_arg('num_points_thresholds', '300') - add_launch_arg('invalid_intensity') - add_launch_arg('frame_id', 'velodyne', 'velodyne frame id') - add_launch_arg('gps_time', 'False') - add_launch_arg('view_direction', description='the center of lidar angle') - add_launch_arg('view_width', description='lidar angle: 0~6.28 [rad]') - add_launch_arg('input_frame', LaunchConfiguration('base_frame'), 'use for cropbox') - add_launch_arg('output_frame', LaunchConfiguration('base_frame'), 'use for cropbox') - add_launch_arg('vehicle_param_file', description='path to the file of vehicle info yaml') - add_launch_arg('vehicle_mirror_param_file', - description='path to the file of vehicle mirror position yaml') - add_launch_arg('use_multithread', 'False', 'use multithread') - add_launch_arg('use_intra_process', 'False', 'use ROS2 component container communication') - - set_container_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container', - condition=UnlessCondition(LaunchConfiguration('use_multithread')) - ) - - set_container_mt_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container_mt', - condition=IfCondition(LaunchConfiguration('use_multithread')) - ) - - return launch.LaunchDescription(launch_arguments + - [set_container_executable, - set_container_mt_executable] + - [OpaqueFunction(function=launch_setup)]) diff --git a/sensing_launch/package.xml b/sensing_launch/package.xml index 13b7bc9276d7a..f5b0339aebb7a 100644 --- a/sensing_launch/package.xml +++ b/sensing_launch/package.xml @@ -10,19 +10,6 @@ ament_cmake_auto - gnss_poser - livox_ros2_driver - pointcloud_preprocessor - tamagawa_imu_driver - topic_tools - ublox_gps - usb_cam - usb_cam - velodyne_driver - velodyne_monitor - velodyne_pointcloud - - ament_lint_auto ament_lint_common