diff --git a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics.param.yaml similarity index 100% rename from aip_x2_launch/config/blockage_diagnostics_param_file.yaml rename to aip_x2_launch/config/blockage_diagnostics.param.yaml diff --git a/aip_x2_launch/config/ring_outlier_filter_node.param.yaml b/aip_x2_launch/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 00000000..76bf6895 --- /dev/null +++ b/aip_x2_launch/config/ring_outlier_filter_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + distance_ratio: 1.03 + object_length_threshold: 0.1 + num_points_threshold: 4 + max_rings_num: 128 + max_points_num_per_ring: 4000 + publish_outlier_pointcloud: false + min_azimuth_deg: 0.0 + max_azimuth_deg: 360.0 + max_distance: 12.0 + vertical_bins: 128 + horizontal_bins: 36 + noise_threshold: 2 diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index d1e79bb7..6fad838f 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -209,11 +209,9 @@ 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")} + ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} else: - ring_outlier_filter_parameters = { - "output_frame": "" - } # keep the output frame as the input frame + ring_outlier_output_frame = {"output_frame": ""} # keep the output frame as the input frame ring_outlier_filter_component = ComposableNode( package="autoware_pointcloud_preprocessor", plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent", @@ -222,7 +220,10 @@ def create_parameter_dict(*args): ("input", "rectified/pointcloud_ex"), ("output", "pointcloud_before_sync"), ], - parameters=[ring_outlier_filter_parameters], + parameters=[ + load_composable_node_param("ring_outlier_filter_node_param_file"), + ring_outlier_output_frame, + ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -342,7 +343,11 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("horizontal_resolution", "0.4") add_launch_arg( "blockage_diagnostics_param_file", - [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"], + [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics.param.yaml"], + ) + add_launch_arg( + "ring_outlier_filter_node_param_file", + [FindPackageShare("aip_x2_launch"), "/config/ring_outlier_filter_node.param.yaml"], ) add_launch_arg( "distortion_corrector_node_param_file", diff --git a/aip_xx1_gen2_description/urdf/sensors.xacro b/aip_xx1_gen2_description/urdf/sensors.xacro index 81faabe8..79c2c15f 100644 --- a/aip_xx1_gen2_description/urdf/sensors.xacro +++ b/aip_xx1_gen2_description/urdf/sensors.xacro @@ -16,24 +16,6 @@ yaw="${calibration['base_link']['sensor_kit_base_link']['yaw']}" /> - - + + + + + + diff --git a/aip_xx1_gen2_launch/launch/imu.launch.xml b/aip_xx1_gen2_launch/launch/imu.launch.xml index 6e835316..5b5d2ed2 100644 --- a/aip_xx1_gen2_launch/launch/imu.launch.xml +++ b/aip_xx1_gen2_launch/launch/imu.launch.xml @@ -36,7 +36,7 @@ - + diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.py b/aip_xx1_gen2_launch/launch/lidar.launch.py index 7174fc9f..81a791b7 100644 --- a/aip_xx1_gen2_launch/launch/lidar.launch.py +++ b/aip_xx1_gen2_launch/launch/lidar.launch.py @@ -1,4 +1,4 @@ -# Copyright 2024 Tier IV, Inc. All rights reserved. +# 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. @@ -92,11 +92,12 @@ def load_yaml(yaml_file_path): path_dictionary = generate_launch_dictionary() base_parameters = {} - base_parameters["host_ip"] = LaunchConfiguration("host_ip") + 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["launch_hw_monitor"] = LaunchConfiguration("launch_hw_monitor").perform(context) base_parameters["vehicle_id"] = LaunchConfiguration("vehicle_id").perform(context) base_parameters["pointcloud_container_name"] = LaunchConfiguration( "pointcloud_container_name" @@ -159,7 +160,7 @@ def generate_launch_description(): config_file_arg = DeclareLaunchArgument( "config_file", default_value=os.path.join( - get_package_share_directory("aip_xx1_gen2_launch"), "config", "lidar_launch.yaml" + get_package_share_directory("aip_xx1_gen2_launch"), "config", "lidar_gen2.yaml" ), description="Path to the configuration file", ) @@ -169,6 +170,7 @@ 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("launch_hw_monitor", "true", description="launch hardware monitor") add_launch_arg("host_ip", "192.168.1.10") add_launch_arg("use_concat_filter", "true") add_launch_arg( diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.xml b/aip_xx1_gen2_launch/launch/lidar.launch.xml deleted file mode 100644 index 4f4b37b7..00000000 --- a/aip_xx1_gen2_launch/launch/lidar.launch.xml +++ /dev/null @@ -1,125 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py index 8d2428d5..224e90a5 100644 --- a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. +# 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. diff --git a/aip_xx1_gen2_launch/launch/sensing.launch.xml b/aip_xx1_gen2_launch/launch/sensing.launch.xml index 74f24c15..87b928b2 100644 --- a/aip_xx1_gen2_launch/launch/sensing.launch.xml +++ b/aip_xx1_gen2_launch/launch/sensing.launch.xml @@ -23,23 +23,26 @@ + + + - + diff --git a/aip_xx1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_xx1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index 80cc7944..856eaa32 100644 --- a/aip_xx1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_xx1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -12,19 +12,34 @@ /**: 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 + # performance monitoring ----> To be confirmed in v0.47.0 + # "blockage: blockage_validation": default # velodyne - velodyne_connection: default - velodyne_temperature: default - velodyne_rpm: default + "velodyne_monitor: velodyne_connection": default + "velodyne_monitor: velodyne_temperature": default + "velodyne_monitor: velodyne_rpm": default + + # gnss + # "connection: gnss_connection": default + # "data: gnss_data": default + # "antenna: gnss_antenna": default + # "tx_usage: gnss_tx_usage": default + # "spoofing: gnss_spoofing": default + # "jamming: gnss_jamming": default + # "fix_topic_status: fix topic status": default + "ublox: fix topic status": default + "ublox: fix": default + + # camera + "topic_state_monitor_camera0: sensing_topic_status": default + "topic_state_monitor_camera1: sensing_topic_status": default + "topic_state_monitor_camera2: sensing_topic_status": default + "topic_state_monitor_camera3: sensing_topic_status": default + "topic_state_monitor_camera4: sensing_topic_status": default + "topic_state_monitor_camera5: sensing_topic_status": default + "topic_state_monitor_camera6: sensing_topic_status": default # imu - gyro_bias_estimator: default + "topic_state_monitor_imu: sensing_topic_status": default + "gyro_bias_validator: gyro_bias_validator": default diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml index 279bf9f2..3a628503 100644 --- a/aip_xx1_launch/launch/lidar.launch.xml +++ b/aip_xx1_launch/launch/lidar.launch.xml @@ -6,7 +6,7 @@ - + @@ -49,7 +49,7 @@ - + @@ -71,7 +71,7 @@ - + @@ -93,7 +93,7 @@ - + diff --git a/common_sensor_launch/config/ring_outlier_filter_node.param.yaml b/common_sensor_launch/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 00000000..76bf6895 --- /dev/null +++ b/common_sensor_launch/config/ring_outlier_filter_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + distance_ratio: 1.03 + object_length_threshold: 0.1 + num_points_threshold: 4 + max_rings_num: 128 + max_points_num_per_ring: 4000 + publish_outlier_pointcloud: false + min_azimuth_deg: 0.0 + max_azimuth_deg: 360.0 + max_distance: 12.0 + vertical_bins: 128 + horizontal_bins: 36 + noise_threshold: 2 diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml index 7d4713fd..dcb260f4 100644 --- a/common_sensor_launch/launch/hesai_OT128.launch.xml +++ b/common_sensor_launch/launch/hesai_OT128.launch.xml @@ -2,6 +2,9 @@ + + @@ -18,6 +21,7 @@ + @@ -32,10 +36,10 @@ - + - - + + diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml index c6ea9b64..e7e93a1b 100644 --- a/common_sensor_launch/launch/hesai_XT32.launch.xml +++ b/common_sensor_launch/launch/hesai_XT32.launch.xml @@ -2,6 +2,9 @@ + + @@ -18,6 +21,7 @@ + @@ -32,9 +36,9 @@ - - - + + + diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index e9660646..66336263 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -25,6 +25,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare import yaml @@ -158,6 +159,45 @@ def create_parameter_dict(*args): ) ) + # There is an issue where hw_monitor crashes due to data race, + # so the monitor will now only be launched when explicitly specified with a launch command. + launch_hw_monitor: bool = IfCondition(LaunchConfiguration("launch_hw_monitor")).evaluate( + context + ) + launch_driver: bool = IfCondition(LaunchConfiguration("launch_driver")).evaluate(context) + if launch_hw_monitor and launch_driver: + 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", + ), + }, + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -221,13 +261,18 @@ def create_parameter_dict(*args): ) ) + ring_outlier_filter_node_param = ParameterFile( + param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").perform(context), + allow_substs=True, + ) + # 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")} + if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true": + ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} else: - ring_outlier_filter_parameters = { - "output_frame": "" - } # keep the output frame as the input frame + # keep the output frame as the input frame + ring_outlier_output_frame = {"output_frame": ""} + nodes.append( ComposableNode( package="autoware_pointcloud_preprocessor", @@ -237,7 +282,7 @@ def create_parameter_dict(*args): ("input", "rectified/pointcloud_ex"), ("output", "pointcloud_before_sync"), ], - parameters=[ring_outlier_filter_parameters], + parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -336,6 +381,11 @@ def add_launch_arg(name: str, default_value=None, description=None): 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( + "launch_hw_monitor", + "False", + "do launch hardware monitor. Due to an issue where hw_monitor crashes due to data conflicts, the monitor in launched only when explicitly specified", + ) 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") @@ -382,6 +432,10 @@ def add_launch_arg(name: str, default_value=None, description=None): "distortion_corrector_node_param_file", [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], ) + add_launch_arg( + "ring_outlier_filter_node_param_file", + [FindPackageShare("common_sensor_launch"), "/config/ring_outlier_filter_node.param.yaml"], + ) set_container_executable = SetLaunchConfiguration( "container_executable",