diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 9ccf52af..2f7df2cb 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -4,207 +4,273 @@ + - + - - + + - - - + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - + + - - + + - - - + + + - - - - + + + + + + + - - - - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - + + + + + + + + + + + + + + + + + + - + @@ -213,8 +279,9 @@ - + + - + \ No newline at end of file diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py new file mode 100644 index 00000000..fdbd72c0 --- /dev/null +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -0,0 +1,378 @@ +# Copyright 2023 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.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_lidar_make(sensor_name): + if sensor_name[:6].lower() == "pandar": + return "Hesai", ".csv" + elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: + return "Velodyne", ".yaml" + return "unrecognized_sensor_model" + + +def get_vehicle_info(context): + # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support + # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py + gp = context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(context.launch_configurations.get("global_params", {})) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = 0.0 + p["max_height_offset"] = gp["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 load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + # Model and make + sensor_model = LaunchConfiguration("sensor_model").perform(context) + sensor_make, sensor_extension = get_lidar_make(sensor_model) + nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") + + # Calibration file + sensor_calib_fp = os.path.join( + nebula_decoders_share_dir, + "calibration", + sensor_make.lower(), + sensor_model + sensor_extension, + ) + assert os.path.exists( + sensor_calib_fp + ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + + nodes = [] + + nodes.append( + ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + ) + + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hardware_monitor_ros_wrapper_node", + parameters=[ + { + "sensor_model": LaunchConfiguration("sensor_model"), + "return_mode": LaunchConfiguration("return_mode"), + "frame_id": LaunchConfiguration("frame_id"), + "scan_phase": LaunchConfiguration("scan_phase"), + "sensor_ip": LaunchConfiguration("sensor_ip"), + "host_ip": LaunchConfiguration("host_ip"), + "data_port": LaunchConfiguration("data_port"), + "gnss_port": LaunchConfiguration("gnss_port"), + "packet_mtu_size": LaunchConfiguration("packet_mtu_size"), + "rotation_speed": LaunchConfiguration("rotation_speed"), + "cloud_min_angle": LaunchConfiguration("cloud_min_angle"), + "cloud_max_angle": LaunchConfiguration("cloud_max_angle"), + "diag_span": LaunchConfiguration("diag_span"), + "dual_return_distance_threshold": LaunchConfiguration("dual_return_distance_threshold"), + "delay_monitor_ms": LaunchConfiguration("delay_monitor_ms"), + }, + ], + condition=IfCondition(LaunchConfiguration("launch_driver")), + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "DriverRosWrapper", + name=sensor_make.lower() + "_driver_ros_wrapper_node", + parameters=[ + { + "calibration_file": sensor_calib_fp, + "sensor_model": sensor_model, + **create_parameter_dict( + "host_ip", + "sensor_ip", + "data_port", + "return_mode", + "min_range", + "max_range", + "frame_id", + "scan_phase", + "cloud_min_angle", + "cloud_max_angle", + "dual_return_distance_threshold", + "gnss_port", + "packet_mtu_size", + "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", + ), + "launch_hw": True + }, + ], + remappings=[ + ("aw_points", "pointcloud_raw"), + ("aw_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_temp/pointcloud_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + mirror_info = load_composable_node_param("vehicle_mirror_param_file") + right = mirror_info["right"] + cropbox_parameters.update( + min_x=right["min_longitudinal_offset"], + max_x=right["max_longitudinal_offset"], + min_y=right["min_lateral_offset"], + max_y=right["max_lateral_offset"], + min_z=right["min_height_offset"], + max_z=right["max_height_offset"], + ) + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror_right", + remappings=[ + ("input", "self_cropped/pointcloud_ex"), + ("output", "right_mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + left = mirror_info["left"] + cropbox_parameters.update( + min_x=left["min_longitudinal_offset"], + max_x=left["max_longitudinal_offset"], + min_y=left["min_lateral_offset"], + max_y=left["max_lateral_offset"], + min_z=left["min_height_offset"], + max_z=left["max_height_offset"], + ) + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror_left", + remappings=[ + ("input", "right_mirror_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="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/output/pointcloud", "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", "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"), + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=nodes, + output="both", + ) + + driver_component = ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwInterfaceRosWrapper", + # node is created in a global context, need to avoid name clash + name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + "calibration_file": sensor_calib_fp, + **create_parameter_dict( + "sensor_ip", + "host_ip", + "scan_phase", + "return_mode", + "frame_id", + "rotation_speed", + "data_port", + "cloud_min_angle", + "cloud_max_angle", + "dual_return_distance_threshold", + "gnss_port", + "packet_mtu_size", + "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", + ), + } + ], + ) + + driver_component_loader = LoadComposableNodes( + composable_node_descriptions=[driver_component], + target_container=container, + condition=IfCondition(LaunchConfiguration("launch_driver")), + ) + + return [container, driver_component_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("sensor_model", description="sensor model name") + add_launch_arg("config_file", "", description="sensor configuration file") + add_launch_arg("launch_driver", "True", "do launch driver") + add_launch_arg("setup_sensor", "False", "configure sensor") + 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("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") + add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device") + add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device") + add_launch_arg("data_port", "2368", "device data port number") + add_launch_arg("gnss_port", "2380", "device gnss port number") + add_launch_arg("packet_mtu_size", "1500", "packet mtu size") + add_launch_arg("rotation_speed", "600", "rotational frequency") + add_launch_arg("dual_return_distance_threshold", "0.1", "dual return distance threshold") + add_launch_arg("frame_id", "lidar", "frame id") + 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_mirror_param_file", description="path to the file of vehicle mirror position yaml" + ) + add_launch_arg("diag_span", "1000") + add_launch_arg("delay_monitor_ms", "2000") + add_launch_arg("use_multithread", "False", "use multithread") + add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") + add_launch_arg("container_name", "nebula_node_container") + + 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)] + ) \ No newline at end of file