From b20cfcd3e2a74112baafe1f018b80ab568df6c7b Mon Sep 17 00:00:00 2001 From: Yuxuan Liu Date: Tue, 11 Jun 2024 18:53:03 +0900 Subject: [PATCH] add pythonic lidar launch; merge radar / imu launch with logic branch Signed-off-by: Yuxuan Liu --- aip_xx1_launch/config/lidar_gen2.yaml | 69 +++++++ aip_xx1_launch/config/lidar_launch.yaml | 70 +++++++ aip_xx1_launch/launch/imu.launch.xml | 25 ++- aip_xx1_launch/launch/lidar.launch.xml.py | 172 ++++++++++++++++++ .../launch/pointcloud_preprocessor.launch.py | 2 +- aip_xx1_launch/launch/radar.launch.xml | 35 +++- aip_xx1_launch/launch/sensing.launch.xml | 2 +- 7 files changed, 362 insertions(+), 13 deletions(-) create mode 100644 aip_xx1_launch/config/lidar_gen2.yaml create mode 100644 aip_xx1_launch/config/lidar_launch.yaml create mode 100644 aip_xx1_launch/launch/lidar.launch.xml.py diff --git a/aip_xx1_launch/config/lidar_gen2.yaml b/aip_xx1_launch/config/lidar_gen2.yaml new file mode 100644 index 00000000..69338fbb --- /dev/null +++ b/aip_xx1_launch/config/lidar_gen2.yaml @@ -0,0 +1,69 @@ +launches: + - sensor_type: hesai_OT128 + namespace: top + parameters: + max_range: 300.0 + sensor_frame: hesai_top + sensor_ip: "192.168.1.201" + data_port: 2368 + scan_phase: 160 + vertical_bins: 128 + - sensor_type: hesai_XT32 + namespace: front_left + parameters: + max_range: 300.0 + sensor_frame: hesai_front_left + sensor_ip: "192.168.1.21" + data_port: 2369 + scan_phase: 50.0 + cloud_min_angle: 50 + cloud_max_angle: 320 + vertical_bins: 16 + horizontal_ring_id: 0 + - sensor_type: hesai_XT32 + namespace: front_right + parameters: + max_range: 300.0 + sensor_frame: hesai_front_right + sensor_ip: "192.168.1.22" + data_port: 2370 + scan_phase: 310.0 + cloud_min_angle: 40 + cloud_max_angle: 310 + - sensor_type: hesai_XT32 + namespace: side_left + parameters: + max_range: 10.0 + sensor_frame: hesai_side_left + sensor_ip: "192.168.1.23" + data_port: 2371 + scan_phase: 90.0 + cloud_min_angle: 90 + cloud_max_angle: 270 + - sensor_type: hesai_XT32 + namespace: side_right + parameters: + max_range: 10.0 + sensor_frame: hesai_side_right + sensor_ip: "192.168.1.24" + data_port: 2372 + scan_phase: 270.0 + cloud_min_angle: 90 + cloud_max_angle: 270 + +preprocessor: + input_topics: + - "/sensing/lidar/top/pointcloud_before_sync" + - "/sensing/lidar/side_left/pointcloud_before_sync" + - "/sensing/lidar/side_right/pointcloud_before_sync" + - "/sensing/lidar/front_left/pointcloud_before_sync" + - "/sensing/lidar/front_right/pointcloud_before_sync" + input_offset: + - 0.035 + - 0.025 + - 0.025 + - 0.025 + - 0.025 + timeout_sec: 0.095 + input_twist_topic_type: "twist" + publish_synchronized_pointcloud: true \ No newline at end of file diff --git a/aip_xx1_launch/config/lidar_launch.yaml b/aip_xx1_launch/config/lidar_launch.yaml new file mode 100644 index 00000000..a8750bc0 --- /dev/null +++ b/aip_xx1_launch/config/lidar_launch.yaml @@ -0,0 +1,70 @@ +launches: + - sensor_type: velodyne_VLS128 + namespace: top + parameters: + max_range: 250.0 + sensor_frame: velodyne_top + sensor_ip: "192.168.1.201" + data_port: 2368 + scan_phase: 300.0 + vertical_bins: 128 + horizontal_ring_id: 64 + horizontal_resolution: 0.4 + is_channel_order_top2down: false + - sensor_type: velodyne_VLP16 + namespace: left + parameters: + max_range: 5.0 + sensor_frame: velodyne_left + sensor_ip: "192.168.1.202" + data_port: 2369 + scan_phase: 180.0 + cloud_min_angle: 300 + cloud_max_angle: 60 + vertical_bins: 16 + horizontal_ring_id: 0 + horizontal_resolution: 0.4 + is_channel_order_top2down: false + - sensor_type: velodyne_VLP16 + namespace: right + parameters: + max_range: 5.0 + sensor_frame: velodyne_right + sensor_ip: "192.168.1.203" + data_port: 2370 + scan_phase: 180.0 + cloud_min_angle: 300 + cloud_max_angle: 60 + vertical_bins: 16 + horizontal_ring_id: 0 + horizontal_resolution: 0.4 + is_channel_order_top2down: false + - sensor_type: velodyne_VLP16 + namespace: rear + parameters: + max_range: 1.5 + sensor_frame: velodyne_rear + sensor_ip: "192.168.1.204" + data_port: 2371 + scan_phase: 180.0 + cloud_min_angle: 300 + cloud_max_angle: 60 + vertical_bins: 16 + horizontal_ring_id: 0 + horizontal_resolution: 0.4 + is_channel_order_top2down: false + +preprocessor: + input_topics: + - "/sensing/lidar/top/pointcloud" + - "/sensing/lidar/left/pointcloud" + - "/sensing/lidar/right/pointcloud" + - "/sensing/lidar/rear/ointcloud" + input_offset: + - 0.035 + - 0.025 + - 0.025 + - 0.025 + timeout_sec: 0.095 + input_twist_topic_type: "twist" + publish_synchronized_pointcloud: false \ No newline at end of file diff --git a/aip_xx1_launch/launch/imu.launch.xml b/aip_xx1_launch/launch/imu.launch.xml index 7f1f0337..e88e8d8e 100644 --- a/aip_xx1_launch/launch/imu.launch.xml +++ b/aip_xx1_launch/launch/imu.launch.xml @@ -1,14 +1,27 @@ + + + + - - - - + + + + + + + + + - + + + + + @@ -21,6 +34,7 @@ + @@ -29,6 +43,7 @@ + diff --git a/aip_xx1_launch/launch/lidar.launch.xml.py b/aip_xx1_launch/launch/lidar.launch.xml.py new file mode 100644 index 00000000..77b861bf --- /dev/null +++ b/aip_xx1_launch/launch/lidar.launch.xml.py @@ -0,0 +1,172 @@ +# Copyright 2024 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. + + +from launch import LaunchDescription +from copy import deepcopy +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration, EnvironmentVariable +from launch_ros.actions import PushRosNamespace +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource +import os +import yaml + + +def generate_launch_dictionary(): + path_dictionary = { + "hesai_OT128": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "hesai_OT128.launch.xml", + ) + ), + "hesai_XT32": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "hesai_XT32.launch.xml", + ) + ), + "velodyne_VLS128": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "velodyne_VLS128.launch.xml", + ) + ), + "velodyne_VLP16": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "velodyne_VLP16.launch.xml", + ) + ), + "livox_horizon": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "livox_horizon.launch.py", + ) + ), + } + return path_dictionary + + +def load_sub_launches_from_yaml(context, *args, **kwargs): + def load_yaml(yaml_file_path): + with open(LaunchConfiguration(yaml_file_path).perform(context), "r") as f: + return yaml.safe_load(f) + + config = load_yaml("config_file") + + path_dictionary = generate_launch_dictionary() + + base_parameters = {} + base_parameters["host_ip"] = LaunchConfiguration("host_ip").perform(context) + base_parameters["vehicle_mirror_param_file"] = LaunchConfiguration( + "vehicle_mirror_param_file" + ).perform(context) + base_parameters["launch_driver"] = LaunchConfiguration("launch_driver").perform(context) + base_parameters["vehicle_id"] = LaunchConfiguration("vehicle_id").perform(context) + base_parameters["pointcloud_container_name"] = LaunchConfiguration( + "pointcloud_container_name" + ).perform(context) + base_parameters["enable_blockage_diag"] = LaunchConfiguration( + "enable_blockage_diag" + ).perform(context) + + sub_launch_actions = [] + for launch in config["launches"]: + launch_parameters = deepcopy(base_parameters) + launch_parameters.update(launch["parameters"]) # dict + launch_parameter_list_tuple = [(str(k),str(v)) for k,v in launch_parameters.items()] + sub_launch_action = GroupAction( + [ + PushRosNamespace(launch["namespace"]), + IncludeLaunchDescription( + deepcopy(path_dictionary[launch["sensor_type"]]), + launch_arguments=launch_parameter_list_tuple, + ), + ] + ) + sub_launch_actions.append(sub_launch_action) + + sub_launch_actions.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("aip_xx1_launch"), + "launch", + "pointcloud_preprocessor.launch.py", + ) + ), + launch_arguments=[ + ("base_frame", "base_link"), + ("use_multithread", "true"), + ("use_intra_process", "true"), + ("use_pointcloud_container", LaunchConfiguration( + "use_pointcloud_container" + )), + ("pointcloud_container_name", LaunchConfiguration( + "pointcloud_container_name" + )) + ] + ) + ) + return [ + GroupAction([PushRosNamespace("lidar"), *sub_launch_actions]), + ] + + +def generate_launch_description(): + # Define launch arguments + launch_arguments = [] + config_file_arg = DeclareLaunchArgument( + "config_file", + default_value=os.path.join( + get_package_share_directory("aip_xx1_launch"), "config", "lidar_launch.yaml" + ), + description="Path to the configuration file", + ) + launch_arguments.append(config_file_arg) + + def add_launch_arg(name: str, default_value=None, **kwargs): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, **kwargs) + ) + + add_launch_arg("launch_driver", "true") + add_launch_arg("host_ip", "192.168.1.10") + add_launch_arg("use_concat_filter", "true") + add_launch_arg( + "vehicle_id", + default_value=EnvironmentVariable("VEHICLE_ID", default_value="default"), + ) + add_launch_arg("vehicle_mirror_param_file") + add_launch_arg( + "use_pointcloud_container", "false", description="launch pointcloud container" + ) + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("enable_blockage_diag", "false") + + # Create launch description with the config_file argument + ld = LaunchDescription(launch_arguments) + # Add sub-launch files dynamically based on the YAML configuration + ld.add_action(OpaqueFunction(function=load_sub_launches_from_yaml)) + + return ld diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index 4aa9736c..6f97dac6 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -23,7 +23,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode - +from launch.launch_description_sources import AnyLaunchDescriptionSource def launch_setup(context, *args, **kwargs): # set concat filter as a component diff --git a/aip_xx1_launch/launch/radar.launch.xml b/aip_xx1_launch/launch/radar.launch.xml index aba667d9..27e2f22d 100644 --- a/aip_xx1_launch/launch/radar.launch.xml +++ b/aip_xx1_launch/launch/radar.launch.xml @@ -1,12 +1,34 @@ - + - - - - + + + + + - + + + + + + + + + + + + + + + + + + + + + + @@ -211,4 +233,5 @@ + diff --git a/aip_xx1_launch/launch/sensing.launch.xml b/aip_xx1_launch/launch/sensing.launch.xml index ba6b935c..44477cae 100644 --- a/aip_xx1_launch/launch/sensing.launch.xml +++ b/aip_xx1_launch/launch/sensing.launch.xml @@ -10,7 +10,7 @@ - +