diff --git a/aip_x1_launch/config/gnss_poser.param.yaml b/aip_x1_launch/config/gnss_poser.param.yaml new file mode 100644 index 00000000..5d4e8fda --- /dev/null +++ b/aip_x1_launch/config/gnss_poser.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + base_frame: base_link + gnss_base_frame: gnss_link + map_frame: map + buff_epoch: 1 + use_gnss_ins_orientation: false + gnss_pose_pub_method: 0 # 0: Median, 1: Average diff --git a/aip_x1_launch/launch/gnss.launch.xml b/aip_x1_launch/launch/gnss.launch.xml index 26b0de4c..ccb484fb 100644 --- a/aip_x1_launch/launch/gnss.launch.xml +++ b/aip_x1_launch/launch/gnss.launch.xml @@ -1,10 +1,9 @@ - - - + + @@ -14,14 +13,13 @@ + - - + + - - diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index dce0a58a..f1796100 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -7,6 +7,7 @@ + @@ -22,6 +23,7 @@ + @@ -41,6 +43,7 @@ + diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index dd2a2c17..eedf65ee 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -36,12 +36,17 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/front_center/pointcloud", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/front_center/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), - "timeout_sec": 1.0, + "input_offset": [ + 0.055, + 0.025, + ], + "timeout_sec": 0.095, "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -66,7 +71,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") - add_launch_arg("container_name", "pointcloud_preprocessor_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index 0f23a733..50d56354 100644 --- a/aip_x1_launch/launch/sensing.launch.xml +++ b/aip_x1_launch/launch/sensing.launch.xml @@ -4,7 +4,7 @@ - + @@ -12,7 +12,6 @@ - @@ -21,7 +20,7 @@ - + diff --git a/aip_x1_launch/package.xml b/aip_x1_launch/package.xml index ae587883..b409beea 100644 --- a/aip_x1_launch/package.xml +++ b/aip_x1_launch/package.xml @@ -10,14 +10,14 @@ ament_cmake_auto + autoware_compare_map_segmentation + autoware_elevation_map_loader + autoware_ground_segmentation + autoware_occupancy_grid_map_outlier_filter + autoware_pointcloud_preprocessor common_sensor_launch - compare_map_segmentation - elevation_map_loader - ground_segmentation imu_corrector individual_params - occupancy_grid_map_outlier_filter - pointcloud_preprocessor rclcpp_components ros2_socketcan tamagawa_imu_driver diff --git a/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index 02ec3b1a..888cd52c 100644 --- a/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -21,7 +21,7 @@ pandar_ptp: default "left_upper: visibility_validation": default blockage_validation: default - concat_status: default + "concatenate_data: concat_status": default sensing_topic_status: default diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 9ccf52af..7d761f44 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -4,207 +4,273 @@ + - + - - + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - + + - - + + - - - - - - - - + + + + + + + + + + + - - - - + + + + + + + + + + - + - - + + - - - - - - - - - - - - - + + + + + + + + + + + - + + + + + + + + + + - + - - + + - - - - - - - - - - - + + + + + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - + + + + + + + - + + + + + + + + + + - + - - + + - - - + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - + - - + + - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - + @@ -213,7 +279,8 @@ - + + 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..7fb66c08 --- /dev/null +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -0,0 +1,369 @@ +# 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) + + 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=[ + { + "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", + "calibration_file", + ), + "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, + **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", + "calibration_file", + ), + } + ], + ) + + 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", "True", "use multithread") + add_launch_arg("use_intra_process", "True", "use ROS 2 component container communication") + add_launch_arg("container_name", "nebula_node_container") + add_launch_arg("calibration_file", "") + + 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/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index ee0291d5..b950761e 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -206,14 +206,22 @@ def create_parameter_dict(*args): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here + if LaunchConfiguration("output_as_sensor_frame").perform(context): + ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + else: + ring_outlier_filter_parameters = { + "output_frame": "" + } # keep the output frame as the input frame ring_outlier_filter_component = ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::RingOutlierFilterComponent", name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], + parameters=[ring_outlier_filter_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -223,7 +231,7 @@ def create_parameter_dict(*args): name="dual_return_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], parameters=[ { @@ -344,6 +352,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("min_azimuth_deg", "135.0") add_launch_arg("max_azimuth_deg", "225.0") add_launch_arg("enable_blockage_diag", "true") + add_launch_arg("output_as_sensor_frame", "True") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py index 6d435dee..3c0a09d8 100644 --- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py @@ -37,19 +37,20 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/front_upper/pointcloud", - "/sensing/lidar/front_lower/pointcloud", - "/sensing/lidar/left_upper/pointcloud", - "/sensing/lidar/left_lower/pointcloud", - "/sensing/lidar/right_upper/pointcloud", - "/sensing/lidar/right_lower/pointcloud", - "/sensing/lidar/rear_upper/pointcloud", - "/sensing/lidar/rear_lower/pointcloud", + "/sensing/lidar/front_upper/pointcloud_before_sync", + "/sensing/lidar/front_lower/pointcloud_before_sync", + "/sensing/lidar/left_upper/pointcloud_before_sync", + "/sensing/lidar/left_lower/pointcloud_before_sync", + "/sensing/lidar/right_upper/pointcloud_before_sync", + "/sensing/lidar/right_lower/pointcloud_before_sync", + "/sensing/lidar/rear_upper/pointcloud_before_sync", + "/sensing/lidar/rear_lower/pointcloud_before_sync", ], "input_offset": [0.025, 0.025, 0.01, 0.0, 0.05, 0.05, 0.05, 0.05], "timeout_sec": 0.075, "output_frame": LaunchConfiguration("base_frame"), "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/aip_x2_launch/package.xml b/aip_x2_launch/package.xml index 65402aa1..b26c7777 100644 --- a/aip_x2_launch/package.xml +++ b/aip_x2_launch/package.xml @@ -10,11 +10,11 @@ ament_cmake_auto + autoware_pointcloud_preprocessor common_sensor_launch dummy_diag_publisher gnss_poser imu_corrector - pointcloud_preprocessor septentrio_gnss_driver tamagawa_imu_driver topic_tools diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt new file mode 100644 index 00000000..549de0f8 --- /dev/null +++ b/aip_xx1_gen2_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_xx1_gen2_description) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_package(INSTALL_TO_SHARE + urdf + config +) diff --git a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml new file mode 100644 index 00000000..88288533 --- /dev/null +++ b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml @@ -0,0 +1,113 @@ +sensor_kit_base_link: + camera0/camera_link: + x: 0.372 # Design Value + y: 0.0 # Design Value + z: -0.207 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + camera1/camera_link: + x: 0.372 # Design Value + y: 0.045 # Design Value + z: -0.207 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + camera2/camera_link: + x: 0.372 # Design Value + y: -0.045 # Design Value + z: -0.207 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + camera3/camera_link: + x: 0.133 # Design Value + y: 0.498 # Design Value + z: -0.246 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.872665 # Design Value + camera4/camera_link: + x: 0.133 # Design Value + y: -0.498 # Design Value + z: -0.246 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -0.872665 # Design Value + camera5/camera_link: + x: 0.095 # Design Value + y: 0.524 # Design Value + z: -0.246 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 1.0472 # Design Value + camera6/camera_link: + x: 0.095 # Design Value + y: -0.524 # Design Value + z: -0.246 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -1.0472 # Design Value + camera7/camera_link: + x: -0.345 # Design Value + y: 0.244 # Design Value + z: -0.174 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 2.70526 # Design Value + camera8/camera_link: + x: -0.345 # Design Value + y: -0.244 # Design Value + z: -0.174 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -2.70526 # Design Value + camera9/camera_link: + x: -0.362 # Design Value + y: 0.202 # Design Value + z: -0.174 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 2.79253 # Design Value + camera10/camera_link: + x: -0.362 # Design Value + y: -0.202 # Design Value + z: -0.174 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -2.79253 # Design Value + hesai_top_base_link: + x: 0.0 # Design Value + y: 0.0 # Design Value + z: 0.0 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 4.36332298038 # Design Value + hesai_left_base_link: + x: 0.0 # Design Value + y: 0.564 # Design Value + z: -0.300 # Design Value + roll: 0.872665 # Design Value + pitch: 0.0 # Design Value + yaw: 3.14159265359 # Design Value + hesai_right_base_link: + x: 0.0 # Design Value + y: -0.564 # Design Value + z: -0.300 # Design Value + roll: 0.69813132679 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + gnss_link: + x: -0.279 # Design Value + y: 0.0 # Design Value + z: -0.160 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + tamagawa/imu_link: + x: -0.129 # Design Value + y: 0.0 # Design Value + z: -0.160 # Design Value + roll: 3.14159265359 + pitch: 0.0 # Design Value + yaw: 3.14159265359 # Design Value diff --git a/aip_xx1_gen2_description/config/sensors_calibration.yaml b/aip_xx1_gen2_description/config/sensors_calibration.yaml new file mode 100644 index 00000000..a57d3ea9 --- /dev/null +++ b/aip_xx1_gen2_description/config/sensors_calibration.yaml @@ -0,0 +1,71 @@ +base_link: + sensor_kit_base_link: + x: 0.9 + y: 0.0 + z: 2.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + hesai_front_left_base_link: + x: 3.373 # Design Value + y: 0.740 # Design Value + z: 0.5482 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 2.44346132679 # Design Value + hesai_front_right_base_link: + x: 3.373 # Design Value + y: -0.740 # Design Value + z: 0.5482 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.69813132679 # Design Value + # velodyne_rear_base_link: #unused + # x: -0.358 + # y: 0.0 + # z: 1.631 + # roll: -0.02 + # pitch: 0.7281317 + # yaw: 3.141592 + front_center/radar_link: + x: 3.520 # Design Value + y: 0.0 # Design Value + z: 0.6352 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + front_right/radar_link: + x: 3.384 # Design Value + y: -0.7775 # Design Value + z: 0.410 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -1.22173 # Design Value + front_left/radar_link: + x: 3.384 # Design Value + y: 0.7775 # Design Value + z: 0.410 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 1.22173 # Design Value + rear_center/radar_link: + x: -0.858 # Design Value + y: 0.0 # Design Value + z: 0.520 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 3.141592 # Design Value + rear_right/radar_link: + x: -0.782 # Design Value + y: -0.761 # Design Value + z: 0.520 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -2.0944 # Design Value + rear_left/radar_link: + x: -0.782 # Design Value + y: 0.761 # Design Value + z: 0.520 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 2.0944 # Design Value diff --git a/aip_xx1_gen2_description/package.xml b/aip_xx1_gen2_description/package.xml new file mode 100644 index 00000000..9b010d72 --- /dev/null +++ b/aip_xx1_gen2_description/package.xml @@ -0,0 +1,17 @@ + + + aip_xx1_gen2_description + 0.1.0 + The aip_xx1_gen2_description package + + Yukihiro Saito + Apache 2 + + ament_cmake_auto + + velodyne_description + + + ament_cmake + + diff --git a/aip_xx1_gen2_description/urdf/sensor_kit.xacro b/aip_xx1_gen2_description/urdf/sensor_kit.xacro new file mode 100644 index 00000000..137b3589 --- /dev/null +++ b/aip_xx1_gen2_description/urdf/sensor_kit.xacro @@ -0,0 +1,253 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_description/urdf/sensors.xacro b/aip_xx1_gen2_description/urdf/sensors.xacro new file mode 100644 index 00000000..81faabe8 --- /dev/null +++ b/aip_xx1_gen2_description/urdf/sensors.xacro @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/CMakeLists.txt b/aip_xx1_gen2_launch/CMakeLists.txt new file mode 100644 index 00000000..7f8d9f60 --- /dev/null +++ b/aip_xx1_gen2_launch/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_xx1_gen2_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + data + config +) diff --git a/aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml new file mode 100644 index 00000000..56fc9dbd --- /dev/null +++ b/aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -0,0 +1,118 @@ +/**: + ros__parameters: + sensing: + type: diagnostic_aggregator/AnalyzerGroup + path: sensing + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": sensing_topic_status"] + timeout: 1.0 + + lidar: + type: diagnostic_aggregator/AnalyzerGroup + path: lidar + analyzers: + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + blockage: + type: diagnostic_aggregator/GenericAnalyzer + path: blockage + contains: [": blockage_validation"] + timeout: 1.0 + velodyne: + type: diagnostic_aggregator/AnalyzerGroup + path: velodyne + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": velodyne_connection"] + timeout: 3.0 + + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": velodyne_temperature"] + timeout: 3.0 + + rpm: + type: diagnostic_aggregator/GenericAnalyzer + path: rpm + contains: [": velodyne_rpm"] + timeout: 3.0 + + gnss: + type: diagnostic_aggregator/AnalyzerGroup + path: gnss + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": gnss_connection"] + timeout: 3.0 + + data: + type: diagnostic_aggregator/GenericAnalyzer + path: data + contains: [": gnss_data"] + timeout: 3.0 + + antenna: + type: diagnostic_aggregator/GenericAnalyzer + path: antenna + contains: [": gnss_antenna"] + timeout: 3.0 + + tx_usage: + type: diagnostic_aggregator/GenericAnalyzer + path: tx_usage + contains: [": gnss_tx_usage"] + timeout: 3.0 + + spoofing: + type: diagnostic_aggregator/GenericAnalyzer + path: spoofing + contains: [": gnss_spoofing"] + timeout: 3.0 + + jamming: + type: diagnostic_aggregator/GenericAnalyzer + path: jamming + contains: [": gnss_jamming"] + timeout: 3.0 + + fix_topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: fix_topic_status + contains: [": fix topic status"] + timeout: 3.0 + + imu: + type: diagnostic_aggregator/AnalyzerGroup + path: imu + analyzers: + bias_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: bias_monitoring + analyzers: + gyro_bias_validator: + type: diagnostic_aggregator/GenericAnalyzer + path: gyro_bias_validator + contains: [": gyro_bias_validator"] + timeout: 1.0 diff --git a/aip_xx1_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_xx1_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml new file mode 100644 index 00000000..80cc7944 --- /dev/null +++ b/aip_xx1_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -0,0 +1,30 @@ +# Description: +# name: diag name +# is_active: Force update or not +# status: diag status set by dummy diag publisher "OK, Warn, Error, Stale" +# +# Note: +# +# default values are: +# is_active: "true" +# status: "OK" +--- +/**: + ros__parameters: + required_diags: + # gnss + gnss_connection: default + gnss_data: default + gnss_antenna: default + gnss_tx_usage: default + gnss_spoofing: default + gnss_jamming: default + fix topic status: default + + # velodyne + velodyne_connection: default + velodyne_temperature: default + velodyne_rpm: default + + # imu + gyro_bias_estimator: default diff --git a/aip_xx1_gen2_launch/config/gyro_bias_estimator.param.yaml b/aip_xx1_gen2_launch/config/gyro_bias_estimator.param.yaml new file mode 100644 index 00000000..d552569f --- /dev/null +++ b/aip_xx1_gen2_launch/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.008 # [rad/s] + timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/aip_xx1_gen2_launch/config/lidar_gen2.yaml b/aip_xx1_gen2_launch/config/lidar_gen2.yaml new file mode 100644 index 00000000..fcfefdd4 --- /dev/null +++ b/aip_xx1_gen2_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 diff --git a/aip_xx1_gen2_launch/config/lidar_launch.yaml b/aip_xx1_gen2_launch/config/lidar_launch.yaml new file mode 100644 index 00000000..c702f766 --- /dev/null +++ b/aip_xx1_gen2_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/pointcloud + input_offset: + - 0.035 + - 0.025 + - 0.025 + - 0.025 + timeout_sec: 0.095 + input_twist_topic_type: twist + publish_synchronized_pointcloud: false diff --git a/aip_xx1_gen2_launch/config/radar_simple_object_merger.param.yaml b/aip_xx1_gen2_launch/config/radar_simple_object_merger.param.yaml new file mode 100644 index 00000000..a817e0b7 --- /dev/null +++ b/aip_xx1_gen2_launch/config/radar_simple_object_merger.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + new_frame_id: "base_link" + timeout_threshold: 1.0 + input_topics: ["/sensing/radar/front_center/detected_objects", "/sensing/radar/front_left/detected_objects", "/sensing/radar/rear_left/detected_objects", "/sensing/radar/rear_center/detected_objects", "/sensing/radar/rear_right/detected_objects", "/sensing/radar/front_right/detected_objects"] diff --git a/aip_xx1_gen2_launch/data/traffic_light_camera.yaml b/aip_xx1_gen2_launch/data/traffic_light_camera.yaml new file mode 100644 index 00000000..458ad17c --- /dev/null +++ b/aip_xx1_gen2_launch/data/traffic_light_camera.yaml @@ -0,0 +1,20 @@ +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/aip_xx1_gen2_launch/launch/camera.launch.xml b/aip_xx1_gen2_launch/launch/camera.launch.xml new file mode 100644 index 00000000..041c70c8 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/camera.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/gnss.launch.xml b/aip_xx1_gen2_launch/launch/gnss.launch.xml new file mode 100644 index 00000000..37ad6bde --- /dev/null +++ b/aip_xx1_gen2_launch/launch/gnss.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/imu.launch.xml b/aip_xx1_gen2_launch/launch/imu.launch.xml new file mode 100644 index 00000000..04011706 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/imu.launch.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.py b/aip_xx1_gen2_launch/launch/lidar.launch.py new file mode 100644 index 00000000..7174fc9f --- /dev/null +++ b/aip_xx1_gen2_launch/launch/lidar.launch.py @@ -0,0 +1,188 @@ +# 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 copy import deepcopy +import os +from typing import Any +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import PushRosNamespace +import yaml + + +def join_list_of_arguments(arguments: List[Any]) -> str: + """Join a list of arguments into a string, used by Include Launch Description. + + Example: + join_list_of_arguments([1,2,3]) -> "[1, 2, 3]" + """ + return f"[{', '.join([str(arg) for arg in arguments])}]" + + +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") + 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) + + processor_dict = config["preprocessor"] + sub_launch_actions.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("aip_xx1_gen2_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")), + ("input_topics", join_list_of_arguments(processor_dict["input_topics"])), + ("input_offset", join_list_of_arguments(processor_dict["input_offset"])), + ("timeout_sec", str(processor_dict["timeout_sec"])), + ("input_twist_topic_type", str(processor_dict["input_twist_topic_type"])), + ( + "publish_synchronized_pointcloud", + str(processor_dict["publish_synchronized_pointcloud"]), + ), + ], + ) + ) + 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_gen2_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_gen2_launch/launch/lidar.launch.xml b/aip_xx1_gen2_launch/launch/lidar.launch.xml new file mode 100644 index 00000000..4f4b37b7 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/lidar.launch.xml @@ -0,0 +1,125 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py new file mode 100644 index 00000000..433b7f58 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,100 @@ +# 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 LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + # set concat filter as a component + concat_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_data", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "concatenated/pointcloud"), + ], + parameters=[ + { + "input_topics": LaunchConfiguration("input_topics"), + "output_frame": LaunchConfiguration("base_frame"), + "input_offset": LaunchConfiguration( + "input_offset" + ), # each sensor will wait 60, 70, 70, 70ms + "timeout_sec": LaunchConfiguration("timeout_sec"), # set shorter than 100ms + "input_twist_topic_type": LaunchConfiguration("input_twist_topic_type"), + "publish_synchronized_pointcloud": LaunchConfiguration( + "publish_synchronized_pointcloud" + ), + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # load concat or passthrough filter + concat_loader = LoadComposableNodes( + composable_node_descriptions=[concat_component], + target_container=LaunchConfiguration("pointcloud_container_name"), + condition=IfCondition(LaunchConfiguration("use_concat_filter")), + ) + + return [concat_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_multithread", "False") + add_launch_arg("use_intra_process", "False") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "concatenate_container") + add_launch_arg( + "input_topics", + "[/sensing/lidar/top/pointcloud, /sensing/lidar/left/pointcloud, /sensing/lidar/right/pointcloud, /sensing/lidar/rear/pointcloud]", + ) + add_launch_arg("input_offset", "[0.035, 0.025, 0.025, 0.025]") + add_launch_arg("timeout_sec", "0.095") + add_launch_arg("input_twist_topic_type", "twist") + add_launch_arg("publish_synchronized_pointcloud", "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/aip_xx1_gen2_launch/launch/radar.launch.xml b/aip_xx1_gen2_launch/launch/radar.launch.xml new file mode 100644 index 00000000..7eb4dbcb --- /dev/null +++ b/aip_xx1_gen2_launch/launch/radar.launch.xml @@ -0,0 +1,236 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/sensing.launch.xml b/aip_xx1_gen2_launch/launch/sensing.launch.xml new file mode 100644 index 00000000..74f24c15 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/sensing.launch.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/package.xml b/aip_xx1_gen2_launch/package.xml new file mode 100644 index 00000000..5934d08a --- /dev/null +++ b/aip_xx1_gen2_launch/package.xml @@ -0,0 +1,32 @@ + + + + aip_xx1_gen2_launch + 0.1.0 + The aip_xx1_gen2_launch package + + Hiroki OTA + Apache License 2.0 + + ament_cmake_auto + + autoware_pointcloud_preprocessor + common_sensor_launch + glog_component + gnss_poser + imu_corrector + pacmod3 + ros2_socketcan + tamagawa_imu_driver + topic_tools + ublox_gps + usb_cam + vehicle_velocity_converter + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index 10542a54..726171f9 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -37,10 +37,10 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/left/pointcloud", - "/sensing/lidar/right/pointcloud", - "/sensing/lidar/rear/pointcloud", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/left/pointcloud_before_sync", + "/sensing/lidar/right/pointcloud_before_sync", + "/sensing/lidar/rear/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), "input_offset": [ @@ -51,6 +51,7 @@ def launch_setup(context, *args, **kwargs): ], # each sensor will wait 60, 70, 70, 70ms "timeout_sec": 0.095, # set shorter than 100ms "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/aip_xx1_launch/package.xml b/aip_xx1_launch/package.xml index 93246e4b..94462318 100644 --- a/aip_xx1_launch/package.xml +++ b/aip_xx1_launch/package.xml @@ -10,12 +10,12 @@ ament_cmake_auto + autoware_pointcloud_preprocessor common_sensor_launch glog_component gnss_poser imu_corrector pacmod3 - pointcloud_preprocessor ros2_socketcan tamagawa_imu_driver topic_tools diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml new file mode 100644 index 00000000..7d4713fd --- /dev/null +++ b/common_sensor_launch/launch/hesai_OT128.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml index 56587f72..c6ea9b64 100644 --- a/common_sensor_launch/launch/hesai_XT32.launch.xml +++ b/common_sensor_launch/launch/hesai_XT32.launch.xml @@ -25,12 +25,18 @@ - - + + + + + + + + diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index eb59ab5d..cd661bab 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -113,6 +113,8 @@ def create_parameter_dict(*args): "cloud_min_angle", "cloud_max_angle", "dual_return_distance_threshold", + "setup_sensor", + "retry_hw", ), }, ], @@ -124,6 +126,37 @@ def create_parameter_dict(*args): ) ) + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + **create_parameter_dict( + "return_mode", + "frame_id", + "scan_phase", + "sensor_ip", + "host_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "rotation_speed", + "cloud_min_angle", + "cloud_max_angle", + "diag_span", + "dual_return_distance_threshold", + "delay_monitor_ms", + ), + }, + ], + condition=IfCondition(LaunchConfiguration("launch_driver")), + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -186,6 +219,13 @@ def create_parameter_dict(*args): ) ) + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here + if LaunchConfiguration("output_as_sensor_frame").perform(context): + ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + else: + ring_outlier_filter_parameters = { + "output_frame": "" + } # keep the output frame as the input frame nodes.append( ComposableNode( package="pointcloud_preprocessor", @@ -193,8 +233,9 @@ def create_parameter_dict(*args): name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], + parameters=[ring_outlier_filter_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -232,6 +273,11 @@ def create_parameter_dict(*args): "packet_mtu_size", "dual_return_distance_threshold", "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", + "retry_hw", ), } ], @@ -289,6 +335,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("config_file", "", description="sensor configuration file") add_launch_arg("launch_driver", "True", "do launch driver") add_launch_arg("setup_sensor", "True", "configure sensor") + add_launch_arg("retry_hw", "false", "retry hw") 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") @@ -311,6 +358,14 @@ def add_launch_arg(name: str, default_value=None, description=None): 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") + add_launch_arg("ptp_profile", "1588v2") + add_launch_arg("ptp_transport_type", "L2") + add_launch_arg("ptp_switch_type", "TSN") + add_launch_arg("ptp_domain", "0") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") + add_launch_arg("diag_span", "1000", "") + add_launch_arg("delay_monitor_ms", "2000", "") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") add_launch_arg("enable_blockage_diag", "true") add_launch_arg("horizontal_ring_id", "64") diff --git a/common_sensor_launch/package.xml b/common_sensor_launch/package.xml index c4b612ff..f6f59192 100644 --- a/common_sensor_launch/package.xml +++ b/common_sensor_launch/package.xml @@ -10,10 +10,10 @@ ament_cmake_auto + autoware_radar_tracks_msgs_converter + autoware_radar_tracks_noise_filter dummy_diag_publisher nebula_sensor_driver - radar_tracks_msgs_converter - radar_tracks_noise_filter velodyne_monitor ament_lint_auto