From d0c36fda19288ce27950c0a830f20c320e833abb Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 25 Jul 2024 11:34:22 +0900 Subject: [PATCH 01/38] feat: add x2 gen2 files (#269) * feat: add aip_x2_gen2 files Signed-off-by: Tomohito ANDO * rename for gen2 Signed-off-by: Tomohito ANDO --------- Signed-off-by: Tomohito ANDO --- aip_x2_gen2_description/CMakeLists.txt | 11 + .../config/sensor_kit_calibration.yaml | 173 +++++++ .../config/sensors_calibration.yaml | 57 +++ aip_x2_gen2_description/package.xml | 17 + aip_x2_gen2_description/urdf/gnss.xacro | 24 + aip_x2_gen2_description/urdf/radar.xacro | 31 ++ aip_x2_gen2_description/urdf/sensor_kit.xacro | 345 ++++++++++++++ aip_x2_gen2_description/urdf/sensors.xacro | 87 ++++ aip_x2_gen2_launch/CMakeLists.txt | 16 + .../blockage_diagnostics_param_file.yaml | 14 + .../sensor_kit.param.yaml | 87 ++++ .../config/dual_return_filter.param.yaml | 13 + .../sensor_kit.param.yaml | 95 ++++ .../config/mosaic_x5_rover.param.yaml | 82 ++++ .../config/simple_object_merger.param.yaml | 6 + .../data/traffic_light_camera.yaml | 20 + aip_x2_gen2_launch/launch/gnss.launch.xml | 29 ++ .../launch/hesai_OT128.launch.xml | 69 +++ .../launch/hesai_QT128.launch.xml | 68 +++ aip_x2_gen2_launch/launch/imu.launch.xml | 43 ++ aip_x2_gen2_launch/launch/lidar.launch.xml | 286 ++++++++++++ .../launch/nebula_node_container.launch.py | 442 ++++++++++++++++++ .../launch/pandar_node_container.launch.py | 364 +++++++++++++++ .../launch/pointcloud_preprocessor.launch.py | 93 ++++ aip_x2_gen2_launch/launch/radar.launch.xml | 214 +++++++++ aip_x2_gen2_launch/launch/sensing.launch.xml | 40 ++ .../launch/topic_state_monitor.launch.py | 356 ++++++++++++++ aip_x2_gen2_launch/package.xml | 31 ++ 28 files changed, 3113 insertions(+) create mode 100644 aip_x2_gen2_description/CMakeLists.txt create mode 100644 aip_x2_gen2_description/config/sensor_kit_calibration.yaml create mode 100644 aip_x2_gen2_description/config/sensors_calibration.yaml create mode 100644 aip_x2_gen2_description/package.xml create mode 100644 aip_x2_gen2_description/urdf/gnss.xacro create mode 100644 aip_x2_gen2_description/urdf/radar.xacro create mode 100644 aip_x2_gen2_description/urdf/sensor_kit.xacro create mode 100644 aip_x2_gen2_description/urdf/sensors.xacro create mode 100644 aip_x2_gen2_launch/CMakeLists.txt create mode 100644 aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml create mode 100644 aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml create mode 100644 aip_x2_gen2_launch/config/dual_return_filter.param.yaml create mode 100644 aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml create mode 100644 aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml create mode 100644 aip_x2_gen2_launch/config/simple_object_merger.param.yaml create mode 100644 aip_x2_gen2_launch/data/traffic_light_camera.yaml create mode 100644 aip_x2_gen2_launch/launch/gnss.launch.xml create mode 100644 aip_x2_gen2_launch/launch/hesai_OT128.launch.xml create mode 100644 aip_x2_gen2_launch/launch/hesai_QT128.launch.xml create mode 100644 aip_x2_gen2_launch/launch/imu.launch.xml create mode 100644 aip_x2_gen2_launch/launch/lidar.launch.xml create mode 100644 aip_x2_gen2_launch/launch/nebula_node_container.launch.py create mode 100644 aip_x2_gen2_launch/launch/pandar_node_container.launch.py create mode 100644 aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py create mode 100644 aip_x2_gen2_launch/launch/radar.launch.xml create mode 100644 aip_x2_gen2_launch/launch/sensing.launch.xml create mode 100644 aip_x2_gen2_launch/launch/topic_state_monitor.launch.py create mode 100644 aip_x2_gen2_launch/package.xml diff --git a/aip_x2_gen2_description/CMakeLists.txt b/aip_x2_gen2_description/CMakeLists.txt new file mode 100644 index 00000000..68dbdf53 --- /dev/null +++ b/aip_x2_gen2_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_x2_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_x2_gen2_description/config/sensor_kit_calibration.yaml b/aip_x2_gen2_description/config/sensor_kit_calibration.yaml new file mode 100644 index 00000000..4a37420d --- /dev/null +++ b/aip_x2_gen2_description/config/sensor_kit_calibration.yaml @@ -0,0 +1,173 @@ +sensor_kit_base_link: + # left upper + left_upper/lidar_base_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + left_lower/lidar_base_link: + x: 0.0 + y: -0.025 + z: -0.115 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + left_front/camera_link: + x: 0.12758 + y: -0.04589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: 0.85478 # 49 [deg] + left_rear/camera_link: + x: -0.12842 + y: -0.04589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: 2.2678 # 130 [deg] + left_center/camera_link: + x: 0.0 + y: -0.04089 + z: -0.18666 + roll: 0.0 + pitch: 0.933278 # 53.50 [deg] + yaw: 1.570796 # 90 [deg] + + # right upper + right_upper/lidar_base_link: + x: 0.0 + y: -2.15178 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 # to turn the connector toward the vehicle + right_lower/lidar_base_link: + x: 0.0 + y: -2.12678 + z: -0.115 + roll: 0.6981 # 40 [deg] + pitch: 0.0 + yaw: 0.0 # to turn the connector toward the vehicle + right_front/camera_link: + x: 0.12758 + y: -2.10589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: -0.85478 # -49 [deg] + right_rear/camera_link: + x: -0.12842 + y: -2.10589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: -2.2678 # -130 [deg] + right_center/camera_link: + x: 0.0 + y: -2.11089 + z: -0.18666 + roll: 0.0 + pitch: 0.933278 # 53.50 [deg] + yaw: -1.570796 # -90 [deg] + + # front upper + top_front_left/imu_link: + x: 0.562 + y: -0.99974 + z: -0.06146 + roll: 3.141592 + pitch: 0.0 + yaw: 0.0 + top_front_right/imu_link: + x: 0.562 + y: -1.13974 + z: -0.06146 + roll: 3.141592 + pitch: 0.0 + yaw: 0.0 + top_front_right/camera_link: + x: 0.73758 + y: -1.26439 + z: -0.06666 + roll: 0.0 + pitch: 0.8373 # 48 [deg] + yaw: 0.0 + top_front_center_right/camera_link: + x: 0.73058 + y: -1.18899 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front_center_left/camera_link: + x: 0.73058 + y: -1.03819 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front_left/camera_link: + x: 0.73058 + y: -0.96279 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front/gnss_link: + x: 0.30133 + y: -1.07589 + z: 0.02861 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # rear upper + top_rear_center/camera_link: + x: -6.06359 + y: -1.20389 + z: -0.14078 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + top_rear/gnss_link: + x: -5.77517 + y: -1.07589 + z: 0.02861 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # front lower + front_upper/lidar_base_link: + x: 0.94058 + y: -1.07589 + z: -1.91826 + roll: 0.0 + pitch: 0.0 + yaw: 1.570796 # 90 [deg] to turn the connector toward the vehicle + front_lower/lidar_base_link: + x: 0.96758 + y: -1.07589 + z: -2.15234 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: 1.570796 # 90 [deg] to turn the connector toward the vehicle + + # rear lower + rear_upper/lidar_base_link: + x: -6.08042 + y: -1.07589 + z: -1.91826 + roll: 0.0 + pitch: 0.0 + yaw: -1.570796 # 90 [deg] to turn the connector toward the vehicle + rear_lower/lidar_base_link: + x: -6.10742 + y: -1.07589 + z: -2.15234 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: -1.570796 # 90 [deg] to turn the connector toward the vehicle diff --git a/aip_x2_gen2_description/config/sensors_calibration.yaml b/aip_x2_gen2_description/config/sensors_calibration.yaml new file mode 100644 index 00000000..1b5105f0 --- /dev/null +++ b/aip_x2_gen2_description/config/sensors_calibration.yaml @@ -0,0 +1,57 @@ +base_link: + sensor_kit_base_link: + x: 4.66244 + y: 1.07589 + z: 2.78926 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # radar + front_center/radar_link: + x: 5.69207 + y: 0.0 + z: 0.78894 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + front_left/radar_link: + x: 5.37204 + y: 1.08537 + z: 0.73431 + roll: 0.0 + pitch: 0.0 + yaw: 1.2211 # 70 [deg] + + front_right/radar_link: + x: 5.37204 + y: -1.06642 + z: 0.73431 + roll: 0.0 + pitch: 0.0 + yaw: -1.2211 # 70 [deg] + + rear_center/radar_link: + x: -1.50704 + y: 0.0 + z: 0.78894 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] + + rear_left/radar_link: + x: -1.27564 + y: 1.07767 + z: 0.5487 + roll: 0.0 + pitch: 0.0 + yaw: 1.9189 # 110 [deg] + + rear_right/radar_link: + x: -1.27564 + y: -1.07768 + z: 0.5487 + roll: 0.0 + pitch: 0.0 + yaw: -1.9189 # 110 [deg] diff --git a/aip_x2_gen2_description/package.xml b/aip_x2_gen2_description/package.xml new file mode 100644 index 00000000..b35ad7a3 --- /dev/null +++ b/aip_x2_gen2_description/package.xml @@ -0,0 +1,17 @@ + + + aip_x2_gen2_description + 0.1.0 + The aip_x2_gen2_description package + + Tomohito Ando + Apache 2 + + ament_cmake_auto + + pandar_description + + + ament_cmake + + diff --git a/aip_x2_gen2_description/urdf/gnss.xacro b/aip_x2_gen2_description/urdf/gnss.xacro new file mode 100644 index 00000000..55002be0 --- /dev/null +++ b/aip_x2_gen2_description/urdf/gnss.xacro @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_description/urdf/radar.xacro b/aip_x2_gen2_description/urdf/radar.xacro new file mode 100644 index 00000000..8b0f8d4b --- /dev/null +++ b/aip_x2_gen2_description/urdf/radar.xacro @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_description/urdf/sensor_kit.xacro b/aip_x2_gen2_description/urdf/sensor_kit.xacro new file mode 100644 index 00000000..2db48d09 --- /dev/null +++ b/aip_x2_gen2_description/urdf/sensor_kit.xacro @@ -0,0 +1,345 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_description/urdf/sensors.xacro b/aip_x2_gen2_description/urdf/sensors.xacro new file mode 100644 index 00000000..34b10729 --- /dev/null +++ b/aip_x2_gen2_description/urdf/sensors.xacro @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/CMakeLists.txt b/aip_x2_gen2_launch/CMakeLists.txt new file mode 100644 index 00000000..f3997ac7 --- /dev/null +++ b/aip_x2_gen2_launch/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_x2_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_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 00000000..e43b2039 --- /dev/null +++ b/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.1 + blockage_count_threshold: 50 + blockage_buffering_frames: 2 + blockage_buffering_interval: 1 + enable_dust_diag: false + publish_debug_image: false + dust_ratio_threshold: 0.2 + dust_count_threshold: 10 + dust_kernel_size: 2 + dust_buffering_frames: 10 + dust_buffering_interval: 1 + blockage_kernel: 10 diff --git a/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml new file mode 100644 index 00000000..6d83d1fc --- /dev/null +++ b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -0,0 +1,87 @@ +/**: + 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 + visibility: + type: diagnostic_aggregator/GenericAnalyzer + path: visibility + contains: ["left_upper: visibility_validation"] + timeout: 1.0 + concat_status: + type: diagnostic_aggregator/GenericAnalyzer + path: concat_status + contains: [": concat_status"] + timeout: 1.0 + pandar: + type: diagnostic_aggregator/AnalyzerGroup + path: pandar + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": pandar_connection"] + timeout: 5.0 + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": pandar_temperature"] + timeout: 5.0 + ptp: + type: diagnostic_aggregator/GenericAnalyzer + path: ptp + contains: [": pandar_ptp"] + timeout: 5.0 + + dust: + type: diagnostic_aggregator/GenericAnalyzer + path: dust + contains: [": dust_validation"] + timeout: 1.0 + + gnss: + type: diagnostic_aggregator/AnalyzerGroup + path: gnss + analyzers: + septentrio: + type: diagnostic_aggregator/AnalyzerGroup + path: septentrio + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + gnss: + type: diagnostic_aggregator/GenericAnalyzer + path: gnss + startswith: ["gnss"] + contains: [": gnss"] + timeout: 5.0 diff --git a/aip_x2_gen2_launch/config/dual_return_filter.param.yaml b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml new file mode 100644 index 00000000..dd68e5ec --- /dev/null +++ b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + roi_mode: "Fixed_azimuth_ROI" # description="options: `No_ROI`, `Fixed_xyz_ROI` or `Fixed_azimuth_ROI`"/> + weak_first_local_noise_threshold: 2 # description="for No_ROI roi_mode, recommended value is 10" /> + visibility_error_threshold: 0.95 + visibility_warn_threshold: 0.97 + max_distance: 5.0 + x_max: 18.0 + x_min: -12.0 + y_max: 2.0 + y_min: -2.0 + z_max: 10.0 + z_min: 0.0 diff --git a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml new file mode 100644 index 00000000..58fdc890 --- /dev/null +++ b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -0,0 +1,95 @@ +# 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 + ## /sensing/gnss/001-connection + "topic_state_monitor_gnss_pose: gnss_topic_status": default + + ## /sensing/gnss/002-quality + "septentrio_driver: Quality indicators": default + + # imu + ## /sensing/imu/001-monitor + "imu_monitor: yaw_rate_status": default + + ## /sensing/imu/002-connection + "topic_state_monitor_imu_data: imu_topic_status": default + + ## /sensing/imu/003-gyro_bias + "gyro_bias_estimator: gyro_bias_validator": default + + # lidar + ## /sensing/lidar/pndr/001-connection + "pandar_monitor: /sensing/lidar/front_lower: pandar_connection": default + "pandar_monitor: /sensing/lidar/front_upper: pandar_connection": default + "pandar_monitor: /sensing/lidar/left_lower: pandar_connection": default + "pandar_monitor: /sensing/lidar/left_upper: pandar_connection": default + "pandar_monitor: /sensing/lidar/right_lower: pandar_connection": default + "pandar_monitor: /sensing/lidar/right_upper: pandar_connection": default + "pandar_monitor: /sensing/lidar/rear_lower: pandar_connection": default + "pandar_monitor: /sensing/lidar/rear_upper: pandar_connection": default + + ## /sensing/lidar/pndr/002-temperature + "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature": default + "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature": default + "pandar_monitor: /sensing/lidar/left_lower: pandar_temperature": default + "pandar_monitor: /sensing/lidar/left_upper: pandar_temperature": default + "pandar_monitor: /sensing/lidar/right_lower: pandar_temperature": default + "pandar_monitor: /sensing/lidar/right_upper: pandar_temperature": default + "pandar_monitor: /sensing/lidar/rear_lower: pandar_temperature": default + "pandar_monitor: /sensing/lidar/rear_upper: pandar_temperature": default + + ## /sensing/lidar/pndr/003-ptp + "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp": default + "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp": default + "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp": default + "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": default + + ## /sensing/camera/001-connection + "topic_state_monitor_camera0: camera0_topic_status": default + "topic_state_monitor_camera1: camera1_topic_status": default + "topic_state_monitor_camera2: camera2_topic_status": default + "topic_state_monitor_camera3: camera3_topic_status": default + "topic_state_monitor_camera4: camera4_topic_status": default + "topic_state_monitor_camera5: camera5_topic_status": default + "topic_state_monitor_camera6: camera6_topic_status": default + "topic_state_monitor_camera7: camera7_topic_status": default + + ## /sensing/radar/001-connection + "topic_state_monitor_radar_front_center: radar_front_center_topic_status": default + "topic_state_monitor_radar_front_left: radar_front_left_topic_status": default + "topic_state_monitor_radar_front_right: radar_front_right_topic_status": default + "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status": default + "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status": default + "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status": default + + ## /others/002-blockage_validation + "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation": default + "blockage_return_diag: /sensing/lidar/front_upper: blockage_validation": default + "blockage_return_diag: /sensing/lidar/left_lower: blockage_validation": default + "blockage_return_diag: /sensing/lidar/left_upper: blockage_validation": default + "blockage_return_diag: /sensing/lidar/right_lower: blockage_validation": default + "blockage_return_diag: /sensing/lidar/right_upper: blockage_validation": default + "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation": default + "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation": default + + ## /others/004-concat_status + "concatenate_data: concat_status": default + + ## /others/005-visibility_validation/front_lower + "dual_return_filter: /sensing/lidar/front_lower: visibility_validation": default diff --git a/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml b/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml new file mode 100644 index 00000000..566ee6ba --- /dev/null +++ b/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + device: tcp://192.168.100.101:28784 + + frame_id: gnss_link + aux1_frame_id: aux1 + get_spatial_config_from_tf: false + use_ros_axis_orientation: false + receiver_type: gnss + multi_antenna: false + + datum: Default + + att_offset: + heading: 0.0 + pitch: 0.0 + + ant_type: Unknown + ant_serial_nr: Unknown + ant_aux1_type: Unknown + ant_aux1_serial_nr: Unknown + + polling_period: + pvt: 200 + rest: 200 + + use_gnss_time: false + + rtk_settings: + ntrip_1: + id: "" + caster: "" + caster_port: 2101 + username: "" + password: "" + mountpoint: "" + version: "v2" + tls: false + fingerprint: "" + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + ip_server_1: + id: "IPS1" + port: 28785 + rtk_standard: "RTCMv3" + send_gga: "auto" + keep_open: true + serial_1: + port: "" + baud_rate: 115200 + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + + publish: + # For both GNSS and INS Rxs + navsatfix: true + gpsfix: false + gpgga: false + gprmc: false + gpst: false + measepoch: false + pvtcartesian: false + pvtgeodetic: true + basevectorcart: false + basevectorgeod: false + poscovcartesian: false + poscovgeodetic: true + velcovgeodetic: false + atteuler: false + attcoveuler: false + pose: false + twist: false + diagnostics: true + # For GNSS Rx only + gpgsa: false + gpgsv: false + + # logger + + activate_debug_log: false diff --git a/aip_x2_gen2_launch/config/simple_object_merger.param.yaml b/aip_x2_gen2_launch/config/simple_object_merger.param.yaml new file mode 100644 index 00000000..a817e0b7 --- /dev/null +++ b/aip_x2_gen2_launch/config/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_x2_gen2_launch/data/traffic_light_camera.yaml b/aip_x2_gen2_launch/data/traffic_light_camera.yaml new file mode 100644 index 00000000..458ad17c --- /dev/null +++ b/aip_x2_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_x2_gen2_launch/launch/gnss.launch.xml b/aip_x2_gen2_launch/launch/gnss.launch.xml new file mode 100644 index 00000000..edcbda19 --- /dev/null +++ b/aip_x2_gen2_launch/launch/gnss.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml new file mode 100644 index 00000000..d4a0a909 --- /dev/null +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml new file mode 100644 index 00000000..ccd227fb --- /dev/null +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/imu.launch.xml b/aip_x2_gen2_launch/launch/imu.launch.xml new file mode 100644 index 00000000..b8a3ee81 --- /dev/null +++ b/aip_x2_gen2_launch/launch/imu.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml new file mode 100644 index 00000000..29ecbc05 --- /dev/null +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -0,0 +1,286 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py new file mode 100644 index 00000000..710f4339 --- /dev/null +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -0,0 +1,442 @@ +# 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 LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +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 + + def str2vector(string): + return [float(x) for x in string.strip("[]").split(",")] + + # 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) + + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + + nebula_ros_hw_monitor_component = 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"), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + nebula_ros_hw_interface_component = ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwInterfaceRosWrapper", + 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", + ), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + nebula_ros_driver_component = 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", + "dual_return_distance_threshold", + ), + "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"] + + self_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + mirror_info = 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"], + ) + + right_mirror_crop_component = 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"], + ) + + left_mirror_crop_component = 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")}], + ) + + undistort_component = 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/velocity_report", "/vehicle/status/velocity_status"), + ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + ring_outlier_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + dual_return_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DualReturnOutlierFilterComponent", + name="dual_return_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + parameters=[ + { + "vertical_bins": LaunchConfiguration("vertical_bins"), + "min_azimuth_deg": LaunchConfiguration("min_azimuth_deg"), + "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"), + } + ] + + [load_composable_node_param("dual_return_filter_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range"))) + blockage_diag_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_return_diag", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "blockage_diag/pointcloud"), + ], + parameters=[ + { + "angle_range": LaunchConfiguration("blockage_range"), + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + "max_distance_range": distance_range[1], + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), + } + ] + + [load_composable_node_param("blockage_diagnostics_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="pandar_node_container", + namespace="pointcloud_preprocessor", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + glog_component, + nebula_ros_driver_component, + self_crop_component, + right_mirror_crop_component, + left_mirror_crop_component, + undistort_component, + ], + ) + + driver_loader = LoadComposableNodes( + composable_node_descriptions=[ + nebula_ros_hw_interface_component, + nebula_ros_hw_monitor_component, + ], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), + ) + + ring_outlier_filter_loader = LoadComposableNodes( + composable_node_descriptions=[ring_outlier_filter_component], + target_container=container, + condition=LaunchConfigurationNotEquals("return_mode", "Dual"), + ) + + dual_return_filter_loader = LoadComposableNodes( + composable_node_descriptions=[dual_return_filter_component], + target_container=container, + condition=LaunchConfigurationEquals("return_mode", "Dual"), + ) + + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) + + return [ + container, + driver_loader, + ring_outlier_filter_loader, + dual_return_filter_loader, + blockage_diag_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", "True", "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") + + add_launch_arg("dual_return_filter_param_file") + add_launch_arg("blockage_diagnostics_param_file") + + add_launch_arg("vertical_bins", "40") + add_launch_arg("horizontal_ring_id", "12") + add_launch_arg("blockage_range", "[270.0, 90.0]") + + add_launch_arg("min_azimuth_deg", "135.0") + add_launch_arg("max_azimuth_deg", "225.0") + add_launch_arg("enable_blockage_diag", "true") + + 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_gen2_launch/launch/pandar_node_container.launch.py b/aip_x2_gen2_launch/launch/pandar_node_container.launch.py new file mode 100644 index 00000000..8d8b13cf --- /dev/null +++ b/aip_x2_gen2_launch/launch/pandar_node_container.launch.py @@ -0,0 +1,364 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +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.actions import Node +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def get_pandar_monitor_info(): + path = os.path.join( + get_package_share_directory("pandar_monitor"), + "config", + "pandar_monitor.param.yaml", + ) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + return p + + +def str2vector(string): + return [float(x) for x in string.strip("[]").split(",")] + + +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.3 # margin to crop pointcloud under vehicle + p["max_height_offset"] = gp["vehicle_height"] + 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 + + monitor_node = Node( + executable="pandar_monitor_node", + package="pandar_monitor", + name="pandar_monitor", + parameters=[ + { + "ip_address": LaunchConfiguration("device_ip"), + } + ] + + [get_pandar_monitor_info()], + condition=IfCondition(LaunchConfiguration("launch_driver")), + output="screen", + ) + + driver_component = ComposableNode( + package="pandar_driver", + plugin="pandar_driver::PandarDriver", + name="pandar_driver", + parameters=[ + { + **create_parameter_dict( + "pcap", "device_ip", "lidar_port", "gps_port", "scan_phase", "model", "frame_id" + ) + } + ], + ) + + pointcloud_component = ComposableNode( + package="pandar_pointcloud", + plugin="pandar_pointcloud::PandarCloud", + name="pandar_cloud", + parameters=[ + { + **create_parameter_dict( + "model", + "scan_phase", + "angle_range", + "distance_range", + "device_ip", + "calibration", + "return_mode", + ) + } + ], + remappings=[("pandar_points", "pointcloud_raw"), ("pandar_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"] + + self_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + mirror_info = 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"], + ) + + right_mirror_crop_component = 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"], + ) + + left_mirror_crop_component = 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")}], + ) + + undistort_component = 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/velocity_report", "/vehicle/status/velocity_status"), + ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + ring_outlier_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + dual_return_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DualReturnOutlierFilterComponent", + name="dual_return_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + parameters=[ + { + "vertical_bins": LaunchConfiguration("vertical_bins"), + "min_azimuth_deg": LaunchConfiguration("min_azimuth_deg"), + "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"), + } + ] + + [load_composable_node_param("dual_return_filter_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range"))) + blockage_diag_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_return_diag", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "blockage_diag/pointcloud"), + ], + parameters=[ + { + "angle_range": LaunchConfiguration("blockage_range"), + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + "max_distance_range": distance_range[1], + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), + } + ] + + [load_composable_node_param("blockage_diagnostics_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="pandar_node_container", + namespace="pointcloud_preprocessor", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + pointcloud_component, + self_crop_component, + right_mirror_crop_component, + left_mirror_crop_component, + undistort_component, + ], + ) + + driver_loader = LoadComposableNodes( + composable_node_descriptions=[driver_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), + ) + + ring_outlier_filter_loader = LoadComposableNodes( + composable_node_descriptions=[ring_outlier_filter_component], + target_container=container, + condition=LaunchConfigurationNotEquals("return_mode", "Dual"), + ) + + dual_return_filter_loader = LoadComposableNodes( + composable_node_descriptions=[dual_return_filter_component], + target_container=container, + condition=LaunchConfigurationEquals("return_mode", "Dual"), + ) + + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) + + return [ + container, + driver_loader, + ring_outlier_filter_loader, + dual_return_filter_loader, + blockage_diag_loader, + monitor_node, + ] + + +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("model") + add_launch_arg("launch_driver", "true") + add_launch_arg("calibration", "") + add_launch_arg("device_ip", "192.168.1.201") + add_launch_arg("scan_phase", "0.0") + add_launch_arg("angle_range", "[270.0, 90.0]") + add_launch_arg("distance_range", "[0.1, 200.0]") + add_launch_arg("return_mode", "Dual") + add_launch_arg("base_frame", "base_link") + add_launch_arg("container_name", "pandar_composable_node_container") + add_launch_arg("pcap", "") + add_launch_arg("lidar_port", "2321") + add_launch_arg("gps_port", "10121") + add_launch_arg("frame_id", "pandar") + add_launch_arg("input_frame", LaunchConfiguration("base_frame")) + add_launch_arg("output_frame", LaunchConfiguration("base_frame")) + add_launch_arg("dual_return_filter_param_file") + add_launch_arg("horizontal_resolution", "0.4") + add_launch_arg( + "blockage_diagnostics_param_file", + [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"], + ) + add_launch_arg("vehicle_mirror_param_file") + add_launch_arg("use_multithread", "true") + add_launch_arg("use_intra_process", "true") + add_launch_arg("vertical_bins", "40") + add_launch_arg("horizontal_ring_id", "12") + add_launch_arg("blockage_range", "[270.0, 90.0]") + + add_launch_arg("min_azimuth_deg", "135.0") + add_launch_arg("max_azimuth_deg", "225.0") + add_launch_arg("enable_blockage_diag", "true") + 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_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py new file mode 100644 index 00000000..ef03f232 --- /dev/null +++ b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,93 @@ +# 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": [ + "/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", + ], + "input_offset": [0.005, 0.025, 0.050, 0.005, 0.050, 0.005, 0.005, 0.025], + "timeout_sec": 0.075, + "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "twist", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + concat_loader = LoadComposableNodes( + composable_node_descriptions=[concat_component], + target_container=LaunchConfiguration("pointcloud_container_name"), + ) + + 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", "True") + add_launch_arg("use_intra_process", "True") + add_launch_arg("pointcloud_container_name", "pointcloud_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)] + ) diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml new file mode 100644 index 00000000..c5214773 --- /dev/null +++ b/aip_x2_gen2_launch/launch/radar.launch.xml @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/sensing.launch.xml b/aip_x2_gen2_launch/launch/sensing.launch.xml new file mode 100644 index 00000000..c50b68be --- /dev/null +++ b/aip_x2_gen2_launch/launch/sensing.launch.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py new file mode 100644 index 00000000..ff3c46d1 --- /dev/null +++ b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py @@ -0,0 +1,356 @@ +# 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 launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # GNSS topic monitor + gnss_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_gnss_pose", + parameters=[ + { + "topic": "/sensing/gnss/pose", + "topic_type": "geometry_msgs/msg/PoseStamped", + "best_effort": True, + "diag_name": "gnss_topic_status", + "warn_rate": 2.5, + "error_rate": 0.5, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # IMU topic monitor + imu_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_imu_data", + parameters=[ + { + "topic": "/sensing/imu/imu_data", + "topic_type": "sensor_msgs/msg/Imu", + "best_effort": True, + "diag_name": "imu_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # Radar topic monitors + radar_front_center_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_center", + parameters=[ + { + "topic": "/sensing/radar/front_center/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_center_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_front_left_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_left", + parameters=[ + { + "topic": "/sensing/radar/front_left/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_left_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_front_right_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_right", + parameters=[ + { + "topic": "/sensing/radar/front_right/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_right_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_center_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_center", + parameters=[ + { + "topic": "/sensing/radar/rear_center/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_center_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_left_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_left", + parameters=[ + { + "topic": "/sensing/radar/rear_left/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_left_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_right_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_right", + parameters=[ + { + "topic": "/sensing/radar/rear_right/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_right_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # Camera topic monitors + camera0_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_camera0", + parameters=[ + { + "topic": "/sensing/camera/camera0/camera_info", + "topic_type": "sensor_msgs/msg/CameraInfo", + "best_effort": True, + "diag_name": "camera0_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + camera1_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_camera1", + parameters=[ + { + "topic": "/sensing/camera/camera1/camera_info", + "topic_type": "sensor_msgs/msg/CameraInfo", + "best_effort": True, + "diag_name": "camera1_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + camera2_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_camera2", + parameters=[ + { + "topic": "/sensing/camera/camera2/camera_info", + "topic_type": "sensor_msgs/msg/CameraInfo", + "best_effort": True, + "diag_name": "camera2_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + camera3_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_camera3", + parameters=[ + { + "topic": "/sensing/camera/camera3/camera_info", + "topic_type": "sensor_msgs/msg/CameraInfo", + "best_effort": True, + "diag_name": "camera3_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + camera4_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_camera4", + parameters=[ + { + "topic": "/sensing/camera/camera4/camera_info", + "topic_type": "sensor_msgs/msg/CameraInfo", + "best_effort": True, + "diag_name": "camera4_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + camera5_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_camera5", + parameters=[ + { + "topic": "/sensing/camera/camera5/camera_info", + "topic_type": "sensor_msgs/msg/CameraInfo", + "best_effort": True, + "diag_name": "camera5_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + camera6_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_camera6", + parameters=[ + { + "topic": "/sensing/camera/camera6/camera_info", + "topic_type": "sensor_msgs/msg/CameraInfo", + "best_effort": True, + "diag_name": "camera6_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + camera7_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_camera7", + parameters=[ + { + "topic": "/sensing/camera/camera7/camera_info", + "topic_type": "sensor_msgs/msg/CameraInfo", + "best_effort": True, + "diag_name": "camera7_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # ComposableNodeContainer to run all ComposableNodes + container = ComposableNodeContainer( + name="topic_state_monitor_container", + namespace="topic_state_monitor", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + gnss_topic_monitor, + imu_topic_monitor, + radar_front_center_monitor, + radar_front_left_monitor, + radar_front_right_monitor, + radar_rear_center_monitor, + radar_rear_left_monitor, + radar_rear_right_monitor, + camera0_topic_monitor, + camera1_topic_monitor, + camera2_topic_monitor, + camera3_topic_monitor, + camera4_topic_monitor, + camera5_topic_monitor, + camera6_topic_monitor, + camera7_topic_monitor, + ], + output="screen", + ) + + return LaunchDescription([container]) diff --git a/aip_x2_gen2_launch/package.xml b/aip_x2_gen2_launch/package.xml new file mode 100644 index 00000000..2743c6a8 --- /dev/null +++ b/aip_x2_gen2_launch/package.xml @@ -0,0 +1,31 @@ + + + + aip_x2_gen2_launch + 0.1.0 + The aip_x2_gen2_launch package + + Tomohito Ando + Apache License 2.0 + + ament_cmake_auto + + common_sensor_launch + dummy_diag_publisher + gnss_poser + imu_corrector + pointcloud_preprocessor + septentrio_gnss_driver + tamagawa_imu_driver + topic_tools + ublox_gps + usb_cam + vehicle_velocity_converter + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + From 133e761008460eb2acc6ccbc1a135f663cabad3c Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 25 Jul 2024 18:54:57 +0900 Subject: [PATCH 02/38] fix: rename aip_x2 to aip_x2_gen2 (#271) Signed-off-by: Tomohito Ando --- aip_x2_gen2_launch/launch/gnss.launch.xml | 2 +- .../launch/hesai_OT128.launch.xml | 6 ++-- .../launch/hesai_QT128.launch.xml | 6 ++-- aip_x2_gen2_launch/launch/imu.launch.xml | 2 +- aip_x2_gen2_launch/launch/lidar.launch.xml | 36 +++++++++---------- .../launch/pandar_node_container.launch.py | 2 +- aip_x2_gen2_launch/launch/radar.launch.xml | 2 +- aip_x2_gen2_launch/launch/sensing.launch.xml | 10 +++--- 8 files changed, 33 insertions(+), 33 deletions(-) diff --git a/aip_x2_gen2_launch/launch/gnss.launch.xml b/aip_x2_gen2_launch/launch/gnss.launch.xml index edcbda19..4dc0f2b8 100644 --- a/aip_x2_gen2_launch/launch/gnss.launch.xml +++ b/aip_x2_gen2_launch/launch/gnss.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index d4a0a909..e8881a1f 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -26,12 +26,12 @@ - + - + - + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index ccd227fb..59df827e 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -27,12 +27,12 @@ - + - + - + diff --git a/aip_x2_gen2_launch/launch/imu.launch.xml b/aip_x2_gen2_launch/launch/imu.launch.xml index b8a3ee81..828d24a7 100644 --- a/aip_x2_gen2_launch/launch/imu.launch.xml +++ b/aip_x2_gen2_launch/launch/imu.launch.xml @@ -22,7 +22,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 29ecbc05..f714d4c3 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -10,13 +10,13 @@ - + - + @@ -27,7 +27,7 @@ - + @@ -50,7 +50,7 @@ - + @@ -58,7 +58,7 @@ - + @@ -81,7 +81,7 @@ - + @@ -89,7 +89,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -122,7 +122,7 @@ - + @@ -147,7 +147,7 @@ - + @@ -156,7 +156,7 @@ - + @@ -179,7 +179,7 @@ - + @@ -187,7 +187,7 @@ - + @@ -210,7 +210,7 @@ - + @@ -218,7 +218,7 @@ - + @@ -243,7 +243,7 @@ - + @@ -251,7 +251,7 @@ - + @@ -276,7 +276,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/pandar_node_container.launch.py b/aip_x2_gen2_launch/launch/pandar_node_container.launch.py index 8d8b13cf..b0a0a13c 100644 --- a/aip_x2_gen2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/pandar_node_container.launch.py @@ -333,7 +333,7 @@ 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_gen2_launch"), "/config/blockage_diagnostics_param_file.yaml"], ) add_launch_arg("vehicle_mirror_param_file") add_launch_arg("use_multithread", "true") diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml index c5214773..c530f2a5 100644 --- a/aip_x2_gen2_launch/launch/radar.launch.xml +++ b/aip_x2_gen2_launch/launch/radar.launch.xml @@ -204,7 +204,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/sensing.launch.xml b/aip_x2_gen2_launch/launch/sensing.launch.xml index c50b68be..63d9c0ab 100644 --- a/aip_x2_gen2_launch/launch/sensing.launch.xml +++ b/aip_x2_gen2_launch/launch/sensing.launch.xml @@ -8,24 +8,24 @@ - + - + - - + @@ -33,7 +33,7 @@ - + From ed51c5ca50bd60bfe80aaf569381acfa06e5a52d Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 30 Aug 2024 18:36:35 +0900 Subject: [PATCH 03/38] chore: update dummy diag for sensors (#295) * chore: update dummy diag for sensors Signed-off-by: Tomohito Ando * ci(pre-commit): autofix --------- Signed-off-by: Tomohito Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../sensor_kit.param.yaml | 68 ++++++------------- 1 file changed, 21 insertions(+), 47 deletions(-) diff --git a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index 58fdc890..b5b53c45 100644 --- a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -13,24 +13,15 @@ ros__parameters: required_diags: # gnss - ## /sensing/gnss/001-connection "topic_state_monitor_gnss_pose: gnss_topic_status": default - - ## /sensing/gnss/002-quality "septentrio_driver: Quality indicators": default # imu - ## /sensing/imu/001-monitor "imu_monitor: yaw_rate_status": default - - ## /sensing/imu/002-connection "topic_state_monitor_imu_data: imu_topic_status": default - - ## /sensing/imu/003-gyro_bias - "gyro_bias_estimator: gyro_bias_validator": default + "gyro_bias_validator: gyro_bias_validator": default # lidar - ## /sensing/lidar/pndr/001-connection "pandar_monitor: /sensing/lidar/front_lower: pandar_connection": default "pandar_monitor: /sensing/lidar/front_upper: pandar_connection": default "pandar_monitor: /sensing/lidar/left_lower: pandar_connection": default @@ -40,7 +31,15 @@ "pandar_monitor: /sensing/lidar/rear_lower: pandar_connection": default "pandar_monitor: /sensing/lidar/rear_upper: pandar_connection": default - ## /sensing/lidar/pndr/002-temperature + "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp": default + "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp": default + "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp": default + "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": defaultlt + "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature": default "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature": default "pandar_monitor: /sensing/lidar/left_lower: pandar_temperature": default @@ -49,47 +48,22 @@ "pandar_monitor: /sensing/lidar/right_upper: pandar_temperature": default "pandar_monitor: /sensing/lidar/rear_lower: pandar_temperature": default "pandar_monitor: /sensing/lidar/rear_upper: pandar_temperature": default + "concatenate_data: concat_status": default - ## /sensing/lidar/pndr/003-ptp - "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp": default - "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp": default - "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp": default - "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": default - - ## /sensing/camera/001-connection - "topic_state_monitor_camera0: camera0_topic_status": default - "topic_state_monitor_camera1: camera1_topic_status": default - "topic_state_monitor_camera2: camera2_topic_status": default - "topic_state_monitor_camera3: camera3_topic_status": default - "topic_state_monitor_camera4: camera4_topic_status": default - "topic_state_monitor_camera5: camera5_topic_status": default - "topic_state_monitor_camera6: camera6_topic_status": default - "topic_state_monitor_camera7: camera7_topic_status": default + # camera + "v4l2_camera_camera0: capture_status": default + "v4l2_camera_camera1: capture_status": default + "v4l2_camera_camera2: capture_status": default + "v4l2_camera_camera3: capture_status": default + "v4l2_camera_camera4: capture_status": default + "v4l2_camera_camera5: capture_status": default + "v4l2_camera_camera6: capture_status": default + "v4l2_camera_camera7: capture_status": default - ## /sensing/radar/001-connection + # radar "topic_state_monitor_radar_front_center: radar_front_center_topic_status": default "topic_state_monitor_radar_front_left: radar_front_left_topic_status": default "topic_state_monitor_radar_front_right: radar_front_right_topic_status": default "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status": default "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status": default "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status": default - - ## /others/002-blockage_validation - "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation": default - "blockage_return_diag: /sensing/lidar/front_upper: blockage_validation": default - "blockage_return_diag: /sensing/lidar/left_lower: blockage_validation": default - "blockage_return_diag: /sensing/lidar/left_upper: blockage_validation": default - "blockage_return_diag: /sensing/lidar/right_lower: blockage_validation": default - "blockage_return_diag: /sensing/lidar/right_upper: blockage_validation": default - "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation": default - "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation": default - - ## /others/004-concat_status - "concatenate_data: concat_status": default - - ## /others/005-visibility_validation/front_lower - "dual_return_filter: /sensing/lidar/front_lower: visibility_validation": default From bd69ec30868d4c514b1685da865a6aa2ce92e79c Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Sat, 31 Aug 2024 17:47:45 +0900 Subject: [PATCH 04/38] chore: sync config files in gen1 to gen2 (#296) Signed-off-by: Tomohito ANDO --- .../sensor_kit.param.yaml | 13 ++ .../config/dual_return_filter.param.yaml | 2 +- .../sensor_kit.param.yaml | 2 +- .../config/gyro_bias_estimator.param.yaml | 6 + .../launch/topic_state_monitor.launch.py | 161 ------------------ 5 files changed, 21 insertions(+), 163 deletions(-) create mode 100644 aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml diff --git a/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 6d83d1fc..f2ffdcbd 100644 --- a/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -85,3 +85,16 @@ startswith: ["gnss"] contains: [": gnss"] timeout: 5.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_x2_gen2_launch/config/dual_return_filter.param.yaml b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml index dd68e5ec..75168e7b 100644 --- a/aip_x2_gen2_launch/config/dual_return_filter.param.yaml +++ b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml @@ -4,7 +4,7 @@ weak_first_local_noise_threshold: 2 # description="for No_ROI roi_mode, recommended value is 10" /> visibility_error_threshold: 0.95 visibility_warn_threshold: 0.97 - max_distance: 5.0 + max_distance: 10.0 x_max: 18.0 x_min: -12.0 y_max: 2.0 diff --git a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index b5b53c45..dd09b44e 100644 --- a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -38,7 +38,7 @@ "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp": default "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp": default "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": defaultlt + "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": default "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature": default "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature": default diff --git a/aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml b/aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml new file mode 100644 index 00000000..d552569f --- /dev/null +++ b/aip_x2_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_x2_gen2_launch/launch/topic_state_monitor.launch.py b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py index ff3c46d1..0eafa9e0 100644 --- a/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py +++ b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py @@ -173,159 +173,6 @@ def generate_launch_description(): extra_arguments=[{"use_intra_process_comms": True}], ) - # Camera topic monitors - camera0_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera0", - parameters=[ - { - "topic": "/sensing/camera/camera0/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera0_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera1_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera1", - parameters=[ - { - "topic": "/sensing/camera/camera1/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera1_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera2_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera2", - parameters=[ - { - "topic": "/sensing/camera/camera2/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera2_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera3_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera3", - parameters=[ - { - "topic": "/sensing/camera/camera3/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera3_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera4_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera4", - parameters=[ - { - "topic": "/sensing/camera/camera4/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera4_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera5_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera5", - parameters=[ - { - "topic": "/sensing/camera/camera5/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera5_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera6_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera6", - parameters=[ - { - "topic": "/sensing/camera/camera6/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera6_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera7_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera7", - parameters=[ - { - "topic": "/sensing/camera/camera7/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera7_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - # ComposableNodeContainer to run all ComposableNodes container = ComposableNodeContainer( name="topic_state_monitor_container", @@ -341,14 +188,6 @@ def generate_launch_description(): radar_rear_center_monitor, radar_rear_left_monitor, radar_rear_right_monitor, - camera0_topic_monitor, - camera1_topic_monitor, - camera2_topic_monitor, - camera3_topic_monitor, - camera4_topic_monitor, - camera5_topic_monitor, - camera6_topic_monitor, - camera7_topic_monitor, ], output="screen", ) From 6a80e76db10d6b2ce05cc986ed5a34434abc358a Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Sat, 31 Aug 2024 17:51:40 +0900 Subject: [PATCH 05/38] chore: fix node container name (#297) Signed-off-by: Tomohito ANDO --- aip_x2_gen2_launch/launch/nebula_node_container.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index 710f4339..67b84ec1 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -324,7 +324,7 @@ def str2vector(string): ) container = ComposableNodeContainer( - name="pandar_node_container", + name="nebula_node_container", namespace="pointcloud_preprocessor", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), From f89f10056f3e2428d895f8086eaa83fd4b261902 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Sat, 31 Aug 2024 17:54:47 +0900 Subject: [PATCH 06/38] chore: remove unused file (#298) Signed-off-by: Tomohito ANDO --- .../launch/pandar_node_container.launch.py | 364 ------------------ 1 file changed, 364 deletions(-) delete mode 100644 aip_x2_gen2_launch/launch/pandar_node_container.launch.py diff --git a/aip_x2_gen2_launch/launch/pandar_node_container.launch.py b/aip_x2_gen2_launch/launch/pandar_node_container.launch.py deleted file mode 100644 index b0a0a13c..00000000 --- a/aip_x2_gen2_launch/launch/pandar_node_container.launch.py +++ /dev/null @@ -1,364 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import LaunchConfigurationEquals -from launch.conditions import LaunchConfigurationNotEquals -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.actions import Node -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def get_pandar_monitor_info(): - path = os.path.join( - get_package_share_directory("pandar_monitor"), - "config", - "pandar_monitor.param.yaml", - ) - with open(path, "r") as f: - p = yaml.safe_load(f)["/**"]["ros__parameters"] - return p - - -def str2vector(string): - return [float(x) for x in string.strip("[]").split(",")] - - -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.3 # margin to crop pointcloud under vehicle - p["max_height_offset"] = gp["vehicle_height"] - 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 - - monitor_node = Node( - executable="pandar_monitor_node", - package="pandar_monitor", - name="pandar_monitor", - parameters=[ - { - "ip_address": LaunchConfiguration("device_ip"), - } - ] - + [get_pandar_monitor_info()], - condition=IfCondition(LaunchConfiguration("launch_driver")), - output="screen", - ) - - driver_component = ComposableNode( - package="pandar_driver", - plugin="pandar_driver::PandarDriver", - name="pandar_driver", - parameters=[ - { - **create_parameter_dict( - "pcap", "device_ip", "lidar_port", "gps_port", "scan_phase", "model", "frame_id" - ) - } - ], - ) - - pointcloud_component = ComposableNode( - package="pandar_pointcloud", - plugin="pandar_pointcloud::PandarCloud", - name="pandar_cloud", - parameters=[ - { - **create_parameter_dict( - "model", - "scan_phase", - "angle_range", - "distance_range", - "device_ip", - "calibration", - "return_mode", - ) - } - ], - remappings=[("pandar_points", "pointcloud_raw"), ("pandar_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"] - - self_crop_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_self", - remappings=[ - ("input", "pointcloud_raw_ex"), - ("output", "self_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - mirror_info = 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"], - ) - - right_mirror_crop_component = 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"], - ) - - left_mirror_crop_component = 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")}], - ) - - undistort_component = 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/velocity_report", "/vehicle/status/velocity_status"), - ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - ring_outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - dual_return_filter_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::DualReturnOutlierFilterComponent", - name="dual_return_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), - ], - parameters=[ - { - "vertical_bins": LaunchConfiguration("vertical_bins"), - "min_azimuth_deg": LaunchConfiguration("min_azimuth_deg"), - "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"), - } - ] - + [load_composable_node_param("dual_return_filter_param_file")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range"))) - blockage_diag_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::BlockageDiagComponent", - name="blockage_return_diag", - remappings=[ - ("input", "pointcloud_raw_ex"), - ("output", "blockage_diag/pointcloud"), - ], - parameters=[ - { - "angle_range": LaunchConfiguration("blockage_range"), - "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), - "vertical_bins": LaunchConfiguration("vertical_bins"), - "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), - "max_distance_range": distance_range[1], - "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), - } - ] - + [load_composable_node_param("blockage_diagnostics_param_file")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - container = ComposableNodeContainer( - name="pandar_node_container", - namespace="pointcloud_preprocessor", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - pointcloud_component, - self_crop_component, - right_mirror_crop_component, - left_mirror_crop_component, - undistort_component, - ], - ) - - driver_loader = LoadComposableNodes( - composable_node_descriptions=[driver_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), - ) - - ring_outlier_filter_loader = LoadComposableNodes( - composable_node_descriptions=[ring_outlier_filter_component], - target_container=container, - condition=LaunchConfigurationNotEquals("return_mode", "Dual"), - ) - - dual_return_filter_loader = LoadComposableNodes( - composable_node_descriptions=[dual_return_filter_component], - target_container=container, - condition=LaunchConfigurationEquals("return_mode", "Dual"), - ) - - blockage_diag_loader = LoadComposableNodes( - composable_node_descriptions=[blockage_diag_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("enable_blockage_diag")), - ) - - return [ - container, - driver_loader, - ring_outlier_filter_loader, - dual_return_filter_loader, - blockage_diag_loader, - monitor_node, - ] - - -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("model") - add_launch_arg("launch_driver", "true") - add_launch_arg("calibration", "") - add_launch_arg("device_ip", "192.168.1.201") - add_launch_arg("scan_phase", "0.0") - add_launch_arg("angle_range", "[270.0, 90.0]") - add_launch_arg("distance_range", "[0.1, 200.0]") - add_launch_arg("return_mode", "Dual") - add_launch_arg("base_frame", "base_link") - add_launch_arg("container_name", "pandar_composable_node_container") - add_launch_arg("pcap", "") - add_launch_arg("lidar_port", "2321") - add_launch_arg("gps_port", "10121") - add_launch_arg("frame_id", "pandar") - add_launch_arg("input_frame", LaunchConfiguration("base_frame")) - add_launch_arg("output_frame", LaunchConfiguration("base_frame")) - add_launch_arg("dual_return_filter_param_file") - add_launch_arg("horizontal_resolution", "0.4") - add_launch_arg( - "blockage_diagnostics_param_file", - [FindPackageShare("aip_x2_gen2_launch"), "/config/blockage_diagnostics_param_file.yaml"], - ) - add_launch_arg("vehicle_mirror_param_file") - add_launch_arg("use_multithread", "true") - add_launch_arg("use_intra_process", "true") - add_launch_arg("vertical_bins", "40") - add_launch_arg("horizontal_ring_id", "12") - add_launch_arg("blockage_range", "[270.0, 90.0]") - - add_launch_arg("min_azimuth_deg", "135.0") - add_launch_arg("max_azimuth_deg", "225.0") - add_launch_arg("enable_blockage_diag", "true") - 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)] - ) From 67d8c7ed4cc35801bf76ef1b01844b221920522f Mon Sep 17 00:00:00 2001 From: OsamuSekino <44186078+OsamuSekino@users.noreply.github.com> Date: Tue, 3 Sep 2024 09:32:29 +0900 Subject: [PATCH 07/38] feat: x2 gen2/asterx sb3 (#286) * replace asterx sb3 * fix ip address * enable gnss.launch --- .../config/asterx_sb3_rover.param.yaml | 82 +++++++++++++++++++ aip_x2_gen2_launch/launch/gnss.launch.xml | 2 +- aip_x2_gen2_launch/launch/sensing.launch.xml | 4 +- 3 files changed, 85 insertions(+), 3 deletions(-) create mode 100644 aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml diff --git a/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml b/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml new file mode 100644 index 00000000..a3e1c9fb --- /dev/null +++ b/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + device: tcp://192.168.20.102:28784 + + frame_id: gnss_link + aux1_frame_id: aux1 + get_spatial_config_from_tf: false + use_ros_axis_orientation: false + receiver_type: gnss + multi_antenna: true + + datum: Default + + att_offset: + heading: 0.0 + pitch: 0.0 + + ant_type: "AERAT1675_382 NONE" + ant_serial_nr: Unknown + ant_aux1_type: "AERAT1675_382 NONE" + ant_aux1_serial_nr: Unknown + + polling_period: + pvt: 200 + rest: 200 + + use_gnss_time: false + + rtk_settings: + ntrip_1: + id: "" + caster: "" + caster_port: 2101 + username: "" + password: "" + mountpoint: "" + version: "v2" + tls: false + fingerprint: "" + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + ip_server_1: + id: "IPS1" + port: 28785 + rtk_standard: "RTCMv3" + send_gga: "auto" + keep_open: true + serial_1: + port: "" + baud_rate: 115200 + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + + publish: + # For both GNSS and INS Rxs + navsatfix: true + gpsfix: false + gpgga: false + gprmc: false + gpst: false + measepoch: false + pvtcartesian: false + pvtgeodetic: true + basevectorcart: false + basevectorgeod: false + poscovcartesian: false + poscovgeodetic: true + velcovgeodetic: false + atteuler: false + attcoveuler: false + pose: false + twist: false + diagnostics: true + # For GNSS Rx only + gpgsa: false + gpgsv: false + + # logger + + activate_debug_log: false diff --git a/aip_x2_gen2_launch/launch/gnss.launch.xml b/aip_x2_gen2_launch/launch/gnss.launch.xml index 4dc0f2b8..8ccd477c 100644 --- a/aip_x2_gen2_launch/launch/gnss.launch.xml +++ b/aip_x2_gen2_launch/launch/gnss.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/sensing.launch.xml b/aip_x2_gen2_launch/launch/sensing.launch.xml index 63d9c0ab..68f655b5 100644 --- a/aip_x2_gen2_launch/launch/sensing.launch.xml +++ b/aip_x2_gen2_launch/launch/sensing.launch.xml @@ -20,9 +20,9 @@ - + From a95536a06dc5739fbe502b8a6502b54559352764 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 4 Sep 2024 11:11:39 +0900 Subject: [PATCH 08/38] chore: fix parameter (#301) Signed-off-by: vividf --- aip_x2_gen2_launch/launch/nebula_node_container.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index 67b84ec1..fa12c01a 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -415,7 +415,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("dual_return_filter_param_file") add_launch_arg("blockage_diagnostics_param_file") - add_launch_arg("vertical_bins", "40") + add_launch_arg("vertical_bins", "128") add_launch_arg("horizontal_ring_id", "12") add_launch_arg("blockage_range", "[270.0, 90.0]") From e084acbee26bd80966a4124c1e38bb380fc89c27 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 12 Sep 2024 15:47:56 +0900 Subject: [PATCH 09/38] fix: imu frame for X2 gen2 (#307) Signed-off-by: Tomohito ANDO --- aip_x2_gen2_launch/launch/imu.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x2_gen2_launch/launch/imu.launch.xml b/aip_x2_gen2_launch/launch/imu.launch.xml index 828d24a7..a2c0c80d 100644 --- a/aip_x2_gen2_launch/launch/imu.launch.xml +++ b/aip_x2_gen2_launch/launch/imu.launch.xml @@ -17,7 +17,7 @@ - + From 0a997b08166c3f4fe53f9a19828a3e6ea598b5c3 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 13 Sep 2024 16:03:56 +0900 Subject: [PATCH 10/38] feat: update launcher to apply nebula updates (#310) * fix(lidar): update PTP settings for new PTP architecture * fix(lidar): set FoVs to their desired (no added padding for correction) values as Nebula handles this now * chore(lidar): change parameters and node name to nebula/develop versions * fix(nebula_node_container): add multicast_ip parameter * fix(nebula_node_container): add point_filters parameter * hotfix(nebula_container_launch): ring outlier filter segfaults, disable it for now * ci(pre-commit): autofix * feat(common_sensor_launch): add distortion corrector param (#247) * feat: add distortion corrector param Signed-off-by: vividf * ci(pre-commit): autofix --------- Signed-off-by: vividf Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * chore: add parameter for distortion corrector --------- Signed-off-by: vividf Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yi-Hsiang Fang (Vivid) <146902905+vividf@users.noreply.github.com> --- aip_x2_gen2_launch/launch/lidar.launch.xml | 88 +++++----- .../launch/nebula_node_container.launch.py | 151 +++++++----------- ...e.yaml => blockage_diagnostics.param.yaml} | 0 .../distortion_corrector_node.param.yaml | 5 + .../launch/nebula_node_container.launch.py | 7 +- 5 files changed, 118 insertions(+), 133 deletions(-) rename common_sensor_launch/config/{blockage_diagnostics_param_file.yaml => blockage_diagnostics.param.yaml} (100%) create mode 100644 common_sensor_launch/config/distortion_corrector_node.param.yaml diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index f714d4c3..c9e0c4af 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -33,12 +33,15 @@ - - - + + + + + + - + @@ -64,12 +67,15 @@ - - - + + + + + + - + @@ -95,14 +101,15 @@ - - - + + + + - + - + @@ -128,14 +135,15 @@ - - - + + + + - + - + @@ -162,12 +170,15 @@ - - - + + + + + + - + @@ -193,12 +204,15 @@ - - - + + + + + + - + @@ -224,14 +238,15 @@ - - - + + + + - + - + @@ -257,14 +272,15 @@ - - - + + + + - + - + diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index fa12c01a..c244fa53 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -19,14 +19,16 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration + +# from launch.conditions import LaunchConfigurationNotEquals from launch.conditions import IfCondition from launch.conditions import LaunchConfigurationEquals -from launch.conditions import LaunchConfigurationNotEquals 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 +from launch_ros.substitutions import FindPackageShare import yaml @@ -99,53 +101,30 @@ def str2vector(string): name="glog_component", ) - nebula_ros_hw_monitor_component = 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"), - }, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - nebula_ros_hw_interface_component = ComposableNode( + nebula_component = ComposableNode( package="nebula_ros", - plugin=sensor_make + "HwInterfaceRosWrapper", - name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + plugin=sensor_make + "RosWrapper", + name=sensor_make.lower() + "_ros_wrapper_node", parameters=[ { - "sensor_model": sensor_model, "calibration_file": sensor_calib_fp, + "sensor_model": sensor_model, + "point_filters": "{}", **create_parameter_dict( - "sensor_ip", "host_ip", - "scan_phase", + "sensor_ip", + "multicast_ip", + "data_port", "return_mode", + "min_range", + "max_range", "frame_id", + "sync_angle", + "cut_angle", + "dual_return_distance_threshold", "rotation_speed", - "data_port", "cloud_min_angle", "cloud_max_angle", - "dual_return_distance_threshold", "gnss_port", "packet_mtu_size", "setup_sensor", @@ -153,37 +132,15 @@ def str2vector(string): "ptp_transport_type", "ptp_switch_type", "ptp_domain", - ), - }, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - nebula_ros_driver_component = 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", - "dual_return_distance_threshold", + "diag_span", ), "launch_hw": True, + "retry_hw": True, }, ], remappings=[ - ("aw_points", "pointcloud_raw"), - ("aw_points_ex", "pointcloud_raw_ex"), + # ("aw_points", "pointcloud_raw"), + ("pandar_points", "pointcloud_raw_ex"), ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -263,23 +220,23 @@ def str2vector(string): remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("~/input/imu", "/sensing/imu/imu_data"), - ("~/input/velocity_report", "/vehicle/status/velocity_status"), ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), + ("~/output/pointcloud", "pointcloud"), ], + parameters=[load_composable_node_param("distortion_corrector_node_param_file")], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - ring_outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) + # ring_outlier_filter_component = ComposableNode( + # package="pointcloud_preprocessor", + # plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + # name="ring_outlier_filter", + # remappings=[ + # ("input", "rectified/pointcloud_ex"), + # ("output", "pointcloud"), + # ], + # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + # ) dual_return_filter_component = ComposableNode( package="pointcloud_preprocessor", @@ -330,7 +287,7 @@ def str2vector(string): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ glog_component, - nebula_ros_driver_component, + nebula_component, self_crop_component, right_mirror_crop_component, left_mirror_crop_component, @@ -338,20 +295,11 @@ def str2vector(string): ], ) - driver_loader = LoadComposableNodes( - composable_node_descriptions=[ - nebula_ros_hw_interface_component, - nebula_ros_hw_monitor_component, - ], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), - ) - - ring_outlier_filter_loader = LoadComposableNodes( - composable_node_descriptions=[ring_outlier_filter_component], - target_container=container, - condition=LaunchConfigurationNotEquals("return_mode", "Dual"), - ) + # ring_outlier_filter_loader = LoadComposableNodes( + # composable_node_descriptions=[ring_outlier_filter_component], + # target_container=container, + # condition=LaunchConfigurationNotEquals("return_mode", "Dual"), + # ) dual_return_filter_loader = LoadComposableNodes( composable_node_descriptions=[dual_return_filter_component], @@ -367,8 +315,7 @@ def str2vector(string): return [ container, - driver_loader, - ring_outlier_filter_loader, + # ring_outlier_filter_loader, dual_return_filter_loader, blockage_diag_loader, ] @@ -388,8 +335,15 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("launch_driver", "True", "do launch driver") add_launch_arg("setup_sensor", "True", "configure sensor") add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") + add_launch_arg( + "multicast_ip", + "", + "the multicast group the sensor shall broadcast to. leave empty to disable multicast", + ) add_launch_arg("host_ip", "255.255.255.255", "host ip address") - add_launch_arg("scan_phase", "0.0") + add_launch_arg("sync_angle", "0") + add_launch_arg("cut_angle", "0.0") + # add_launch_arg("point_filters", "{}", "point filter definitions in JSON format") 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") @@ -407,14 +361,19 @@ def add_launch_arg(name: str, default_value=None, description=None): "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") add_launch_arg("dual_return_filter_param_file") - add_launch_arg("blockage_diagnostics_param_file") - + add_launch_arg( + "blockage_diagnostics_param_file", + [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics.param.yaml"], + ) + add_launch_arg( + "distortion_corrector_node_param_file", + [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], + ) add_launch_arg("vertical_bins", "128") add_launch_arg("horizontal_ring_id", "12") add_launch_arg("blockage_range", "[270.0, 90.0]") diff --git a/common_sensor_launch/config/blockage_diagnostics_param_file.yaml b/common_sensor_launch/config/blockage_diagnostics.param.yaml similarity index 100% rename from common_sensor_launch/config/blockage_diagnostics_param_file.yaml rename to common_sensor_launch/config/blockage_diagnostics.param.yaml diff --git a/common_sensor_launch/config/distortion_corrector_node.param.yaml b/common_sensor_launch/config/distortion_corrector_node.param.yaml new file mode 100644 index 00000000..3afa4816 --- /dev/null +++ b/common_sensor_launch/config/distortion_corrector_node.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + base_frame: base_link + use_imu: true + use_3d_distortion_correction: false diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index c3339e0c..b083c0f0 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -182,6 +182,7 @@ def create_parameter_dict(*args): ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), ("~/output/pointcloud", "rectified/pointcloud_ex"), ], + parameters=[load_composable_node_param("distortion_corrector_node_param_file")], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -328,7 +329,11 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("horizontal_resolution", "0.4") add_launch_arg( "blockage_diagnostics_param_file", - [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics_param_file.yaml"], + [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics.param.yaml"], + ) + add_launch_arg( + "distortion_corrector_node_param_file", + [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], ) set_container_executable = SetLaunchConfiguration( From 9688161ea6e37ab3b000472337509e923a1d7de4 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 13 Sep 2024 16:56:19 +0900 Subject: [PATCH 11/38] fix: remove duplicated parameter (#312) * remove duplicated parameter * use pointcloud container var --- aip_x2_gen2_launch/launch/lidar.launch.xml | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index c9e0c4af..203845ce 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -21,7 +21,7 @@ - + @@ -44,7 +44,7 @@ - + @@ -55,7 +55,6 @@ - @@ -78,7 +77,7 @@ - + @@ -89,7 +88,6 @@ - @@ -112,7 +110,7 @@ - + @@ -123,7 +121,6 @@ - @@ -146,7 +143,7 @@ - + @@ -157,7 +154,6 @@ - @@ -192,7 +188,6 @@ - @@ -226,7 +221,6 @@ - @@ -260,7 +254,6 @@ - @@ -294,7 +287,6 @@ - From f07001feab1626c2cb64576b8bce0aeebf75d3ae Mon Sep 17 00:00:00 2001 From: OsamuSekino <44186078+OsamuSekino@users.noreply.github.com> Date: Mon, 16 Sep 2024 09:20:50 +0900 Subject: [PATCH 12/38] fix: gnss antenna (#311) fix gnss antenna --- aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml b/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml index a3e1c9fb..559b7ea5 100644 --- a/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml +++ b/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml @@ -15,9 +15,9 @@ heading: 0.0 pitch: 0.0 - ant_type: "AERAT1675_382 NONE" + ant_type: "SEPPOLANT_MC.V2 NONE" ant_serial_nr: Unknown - ant_aux1_type: "AERAT1675_382 NONE" + ant_aux1_type: "SEPPOLANT_MC.V2 NONE" ant_aux1_serial_nr: Unknown polling_period: From 3a457bddc88c49e261ced0ab3c8b837df7f46b14 Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Wed, 18 Sep 2024 08:25:02 +0900 Subject: [PATCH 13/38] chore(nebula_node_container): fix, confirm and re-enable ring_outlier_filter (#316) * chore(nebula_node_container): confirm ring outlier filter is working with the latest versions: nebula: develop-eval@81afef9ec1cdc1dae9d39e08e916813baab1fd4c transport_drivers: autowarefoundation/transport_drivers.git@8a93ae022604df8132bd65ab8a599532cf4c0a98 ros2_socketcan: /autowarefoundation/ros2_socketcan.git@2bbdba46ce4177e3076892aa99ede557b4f82446 * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../launch/nebula_node_container.launch.py | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index c244fa53..e09ff11a 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -23,6 +23,7 @@ # from launch.conditions import LaunchConfigurationNotEquals from launch.conditions import IfCondition from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer @@ -221,22 +222,22 @@ def str2vector(string): ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), - ("~/output/pointcloud", "pointcloud"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), ], parameters=[load_composable_node_param("distortion_corrector_node_param_file")], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # ring_outlier_filter_component = ComposableNode( - # package="pointcloud_preprocessor", - # plugin="pointcloud_preprocessor::RingOutlierFilterComponent", - # name="ring_outlier_filter", - # remappings=[ - # ("input", "rectified/pointcloud_ex"), - # ("output", "pointcloud"), - # ], - # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - # ) + ring_outlier_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) dual_return_filter_component = ComposableNode( package="pointcloud_preprocessor", @@ -295,11 +296,11 @@ def str2vector(string): ], ) - # ring_outlier_filter_loader = LoadComposableNodes( - # composable_node_descriptions=[ring_outlier_filter_component], - # target_container=container, - # condition=LaunchConfigurationNotEquals("return_mode", "Dual"), - # ) + ring_outlier_filter_loader = LoadComposableNodes( + composable_node_descriptions=[ring_outlier_filter_component], + target_container=container, + condition=LaunchConfigurationNotEquals("return_mode", "Dual"), + ) dual_return_filter_loader = LoadComposableNodes( composable_node_descriptions=[dual_return_filter_component], @@ -315,7 +316,7 @@ def str2vector(string): return [ container, - # ring_outlier_filter_loader, + ring_outlier_filter_loader, dual_return_filter_loader, blockage_diag_loader, ] From 6f863e3695fcef8848dbf7c5af34b6eeb9113a99 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 4 Oct 2024 11:42:43 +0900 Subject: [PATCH 14/38] feat: read LiDAR calibration params from file (#322) * feat: read LiDAR calibration params from file * fix pre-commit errors Signed-off-by: Tomohito ANDO * fix pre-commit errors Signed-off-by: Tomohito ANDO --------- Signed-off-by: Tomohito ANDO --- .../launch/hesai_OT128.launch.xml | 4 ++-- .../launch/hesai_QT128.launch.xml | 4 ++-- aip_x2_gen2_launch/launch/lidar.launch.xml | 16 ++++++++-------- .../launch/nebula_node_container.launch.py | 19 +++---------------- 4 files changed, 15 insertions(+), 28 deletions(-) diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index e8881a1f..1c5a2707 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -26,7 +26,7 @@ - + @@ -61,7 +61,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index 59df827e..4a29e9c0 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -27,7 +27,7 @@ - + @@ -61,7 +61,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 203845ce..6655c3d8 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -53,7 +53,7 @@ - + @@ -86,7 +86,7 @@ - + @@ -119,7 +119,7 @@ - + @@ -152,7 +152,7 @@ - + @@ -186,7 +186,7 @@ - + @@ -219,7 +219,7 @@ - + @@ -252,7 +252,7 @@ - + @@ -285,7 +285,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index e09ff11a..3e863232 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -12,9 +12,6 @@ # 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 @@ -83,18 +80,6 @@ def str2vector(string): # 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) glog_component = ComposableNode( package="glog_component", @@ -108,7 +93,6 @@ def str2vector(string): name=sensor_make.lower() + "_ros_wrapper_node", parameters=[ { - "calibration_file": sensor_calib_fp, "sensor_model": sensor_model, "point_filters": "{}", **create_parameter_dict( @@ -134,6 +118,7 @@ def str2vector(string): "ptp_switch_type", "ptp_domain", "diag_span", + "calibration_file", ), "launch_hw": True, "retry_hw": True, @@ -383,6 +368,8 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("max_azimuth_deg", "225.0") add_launch_arg("enable_blockage_diag", "true") + add_launch_arg("calibration_file", "") + set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", From efb3773402a21da3b0fc488b385fa20fbbf9e524 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Sat, 5 Oct 2024 17:16:43 +0900 Subject: [PATCH 15/38] fix: pass launch_driver argument (#325) * fix: pass launch_driver argument Signed-off-by: Tomohito ANDO * ci(pre-commit): autofix --------- Signed-off-by: Tomohito ANDO Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x2_gen2_launch/launch/hesai_OT128.launch.xml | 2 +- aip_x2_gen2_launch/launch/hesai_QT128.launch.xml | 2 +- aip_x2_gen2_launch/launch/nebula_node_container.launch.py | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index 1c5a2707..cfc4f138 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -32,7 +32,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index 4a29e9c0..9f48bfae 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -33,7 +33,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index 3e863232..49ec3779 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -119,8 +119,8 @@ def str2vector(string): "ptp_domain", "diag_span", "calibration_file", + "launch_hw", ), - "launch_hw": True, "retry_hw": True, }, ], @@ -318,7 +318,7 @@ 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", "True", "do launch driver") add_launch_arg("setup_sensor", "True", "configure sensor") add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") add_launch_arg( From 61043354be6e540d97b7356656bf49955cb6e304 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 8 Oct 2024 22:36:12 +0900 Subject: [PATCH 16/38] fix: make sync angle and cutting angle same as gen1 (#323) * chore: change sync angle to be forward Signed-off-by: Tomohito ANDO * change cut_angle of rear LiDAR Signed-off-by: Tomohito ANDO --------- Signed-off-by: Tomohito ANDO --- aip_x2_gen2_launch/launch/lidar.launch.xml | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 6655c3d8..8f1c219a 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -33,8 +33,8 @@ - - + + @@ -66,7 +66,7 @@ - + @@ -99,8 +99,8 @@ - - + + @@ -132,7 +132,7 @@ - + @@ -166,7 +166,7 @@ - + @@ -199,7 +199,7 @@ - + @@ -232,7 +232,7 @@ - + @@ -265,7 +265,7 @@ - + From ebd4d86145b250d46de8fc6eecab34146e11ef29 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Wed, 9 Oct 2024 08:47:15 +0900 Subject: [PATCH 17/38] feat: filter points by ring_section_filter in nebula (#329) * set point_filters test * fix ring id * add point_filters params * add default point_filter(no reduction) * one third all OT128 * set default no reduction * fix Signed-off-by: Tomohito ANDO --------- Signed-off-by: Tomohito ANDO Co-authored-by: Tomohito ANDO --- .../config/point_filters_full.param.yaml | 5 + .../config/point_filters_one_half.param.yaml | 71 ++++++++++++++ .../config/point_filters_one_third.param.yaml | 92 +++++++++++++++++++ .../config/point_filters_two_third.param.yaml | 50 ++++++++++ .../launch/hesai_OT128.launch.xml | 2 + .../launch/hesai_QT128.launch.xml | 2 + aip_x2_gen2_launch/launch/lidar.launch.xml | 4 + .../launch/nebula_node_container.launch.py | 5 +- 8 files changed, 229 insertions(+), 2 deletions(-) create mode 100644 aip_x2_gen2_launch/config/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters_one_half.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters_two_third.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters_full.param.yaml new file mode 100644 index 00000000..7ff58606 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters_full.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + point_filters: | + { + } diff --git a/aip_x2_gen2_launch/config/point_filters_one_half.param.yaml b/aip_x2_gen2_launch/config/point_filters_one_half.param.yaml new file mode 100644 index 00000000..42db0ed7 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters_one_half.param.yaml @@ -0,0 +1,71 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [1, 0, 360], + [3, 0, 360], + [5, 0, 360], + [7, 0, 360], + [9, 0, 360], + [11, 0, 360], + [13, 0, 360], + [15, 0, 360], + [17, 0, 360], + [19, 0, 360], + [21, 0, 360], + [23, 0, 360], + [25, 0, 360], + [27, 0, 360], + [29, 0, 360], + [31, 0, 360], + [33, 0, 360], + [35, 0, 360], + [37, 0, 360], + [39, 0, 360], + [41, 0, 360], + [43, 0, 360], + [45, 0, 360], + [47, 0, 360], + [49, 0, 360], + [51, 0, 360], + [53, 0, 360], + [55, 0, 360], + [57, 0, 360], + [59, 0, 360], + [61, 0, 360], + [63, 0, 360], + [65, 0, 360], + [67, 0, 360], + [69, 0, 360], + [71, 0, 360], + [73, 0, 360], + [75, 0, 360], + [77, 0, 360], + [79, 0, 360], + [81, 0, 360], + [83, 0, 360], + [85, 0, 360], + [87, 0, 360], + [89, 0, 360], + [91, 0, 360], + [93, 0, 360], + [95, 0, 360], + [97, 0, 360], + [99, 0, 360], + [101, 0, 360], + [103, 0, 360], + [105, 0, 360], + [107, 0, 360], + [109, 0, 360], + [111, 0, 360], + [113, 0, 360], + [115, 0, 360], + [117, 0, 360], + [119, 0, 360], + [121, 0, 360], + [123, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters_one_third.param.yaml new file mode 100644 index 00000000..075381cc --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters_one_third.param.yaml @@ -0,0 +1,92 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters_two_third.param.yaml new file mode 100644 index 00000000..45a57f55 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters_two_third.param.yaml @@ -0,0 +1,50 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index cfc4f138..6315c1b3 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -30,6 +30,7 @@ + @@ -64,6 +65,7 @@ + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index 9f48bfae..8b0276d5 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -31,6 +31,7 @@ + @@ -64,5 +65,6 @@ + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 8f1c219a..6c68f439 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -55,6 +55,7 @@ + @@ -88,6 +89,7 @@ + @@ -188,6 +190,7 @@ + @@ -221,6 +224,7 @@ + diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index 49ec3779..f76911eb 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -94,7 +94,6 @@ def str2vector(string): parameters=[ { "sensor_model": sensor_model, - "point_filters": "{}", **create_parameter_dict( "host_ip", "sensor_ip", @@ -123,7 +122,8 @@ def str2vector(string): ), "retry_hw": True, }, - ], + ] + + [load_composable_node_param("point_filters_param_file")], remappings=[ # ("aw_points", "pointcloud_raw"), ("pandar_points", "pointcloud_raw_ex"), @@ -367,6 +367,7 @@ def add_launch_arg(name: str, default_value=None, description=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("point_filters_param_file") add_launch_arg("calibration_file", "") From 9460e1561dedb66b4bf0e99e51f12bb9466d186b Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Sat, 12 Oct 2024 09:59:00 +0900 Subject: [PATCH 18/38] feat: add point filters to remove noise due to reflection (#331) * feat: add point filters to remove noise due to reflection Signed-off-by: Tomohito ANDO * fix Signed-off-by: Tomohito ANDO * filter points under ground Signed-off-by: Tomohito ANDO --------- Signed-off-by: Tomohito ANDO --- .../point_filters_left_upper.param.yaml | 47 +++++++++++++++++++ .../point_filters_right_upper.param.yaml | 42 +++++++++++++++++ aip_x2_gen2_launch/launch/lidar.launch.xml | 4 +- 3 files changed, 91 insertions(+), 2 deletions(-) create mode 100644 aip_x2_gen2_launch/config/point_filters_left_upper.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters_right_upper.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters_left_upper.param.yaml b/aip_x2_gen2_launch/config/point_filters_left_upper.param.yaml new file mode 100644 index 00000000..1d5d525b --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters_left_upper.param.yaml @@ -0,0 +1,47 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [15, 143.95, 147.1], + [72, 319, 322], + [73, 318, 323], + [74, 318, 323], + [75, 318, 323], + [76, 318, 324], + [77, 318, 325], + [77, 318, 325], + [78, 318, 325], + [79, 318, 325], + [80, 318, 325], + [81, 318, 325], + [82, 318, 325], + [83, 318, 325], + [84, 319, 325], + [85, 319, 325], + [86, 319, 325], + [87, 320, 325], + [88, 321, 325], + [89, 322, 323], + + [91, 287.55, 289.90], + [92, 287.55, 289.75], + [109, 314.74, 316.74], + [110, 312.74, 317.34], + [111, 312.73, 315.93], + [112, 308.48, 313.28], + [113, 305.68, 312.28], + [114, 308.58, 310.58], + [117, 305.77, 313.77], + [118, 303.81, 308.53], + [119, 302.09, 307.53], + [120, 298.42, 307.63], + [121, 299.63, 312.03], + [122, 298.42, 310.02], + [123, 298.42, 310.22], + [124, 298.16, 308.76], + [125, 295.94, 307.54], + [126, 292.12, 303.92], + [127, 289.71, 297.31] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters_right_upper.param.yaml b/aip_x2_gen2_launch/config/point_filters_right_upper.param.yaml new file mode 100644 index 00000000..d47a15dd --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters_right_upper.param.yaml @@ -0,0 +1,42 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [71, 36, 39], + [72, 36, 39], + [73, 36, 39], + [74, 36, 41], + [75, 36, 41], + [76, 36, 41], + [77, 34, 41], + [77, 34, 41], + [78, 33, 41], + [79, 33, 41], + [80, 33, 41], + [81, 33, 41], + [82, 33, 41], + [83, 33, 41], + [84, 33, 40], + [85, 33, 40], + [86, 33, 39], + [87, 33, 39], + [88, 34, 37], + [88, 36, 37], + [88, 35, 37], + + [110, 43.17, 45.17], + [110, 87.78, 89.97], + [117, 49.80, 51.80], + [118, 49.60, 51.60], + [119, 50.40, 52.60], + [120, 49.28, 52.88], + [121, 46.08, 55.08], + [122, 48.07, 56.87], + [123, 48.07, 58.67], + [124, 51.80, 60.20], + [125, 50.98, 63.58], + [126, 52.56, 65.76], + [127, 58.57, 69.17] + ] + } diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 6c68f439..d81914fb 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -89,7 +89,7 @@ - + @@ -224,7 +224,7 @@ - + From 9e9d11a8722d49566d36ebc7f5a15019b9445f3a Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 15 Oct 2024 10:55:20 +0900 Subject: [PATCH 19/38] fix: fov for front_upper lidar (#332) Signed-off-by: Tomohito Ando --- aip_x2_gen2_launch/launch/lidar.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index d81914fb..5224e317 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -170,7 +170,7 @@ - + From 3013e905f8052bb84c3a07224112b47f4b582182 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 22 Oct 2024 13:59:37 +0900 Subject: [PATCH 20/38] feat(aip_x2_gen2_launch): add launcher for ARS548 to pass some missing arguments (#334) * feat: add launcher for radar to pass some missing arguments Signed-off-by: Tomohito Ando * remove unuesed transform node Signed-off-by: Tomohito Ando * remove unused parameter Signed-off-by: Tomohito Ando * Revert "remove unuesed transform node" This reverts commit 87b5351154c582911c4b0fcfda8eedf79242a47c. * remove unnecessary comments Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando --- aip_x2_gen2_launch/config/ARS548.param.yaml | 17 ++++++++++ aip_x2_gen2_launch/launch/ars548.launch.xml | 23 +++++++++++++ aip_x2_gen2_launch/launch/radar.launch.xml | 37 ++++++++------------- 3 files changed, 53 insertions(+), 24 deletions(-) create mode 100644 aip_x2_gen2_launch/config/ARS548.param.yaml create mode 100644 aip_x2_gen2_launch/launch/ars548.launch.xml diff --git a/aip_x2_gen2_launch/config/ARS548.param.yaml b/aip_x2_gen2_launch/config/ARS548.param.yaml new file mode 100644 index 00000000..1fe5e7e2 --- /dev/null +++ b/aip_x2_gen2_launch/config/ARS548.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + host_ip: 10.13.1.166 + sensor_ip: 10.13.1.114 + data_port: 42102 + base_frame: base_link + object_frame: base_link + launch_hw: true + multicast_ip: 224.0.2.2 + sensor_model: ARS548 + configuration_host_port: 42401 + configuration_sensor_port: 42101 + use_sensor_time: false + configuration_vehicle_length: 7.2369 + configuration_vehicle_width: 2.2916 + configuration_vehicle_height: 3.08 + configuration_vehicle_wheelbase: 4.76012 diff --git a/aip_x2_gen2_launch/launch/ars548.launch.xml b/aip_x2_gen2_launch/launch/ars548.launch.xml new file mode 100644 index 00000000..29c03ecb --- /dev/null +++ b/aip_x2_gen2_launch/launch/ars548.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml index c530f2a5..5fc16689 100644 --- a/aip_x2_gen2_launch/launch/radar.launch.xml +++ b/aip_x2_gen2_launch/launch/radar.launch.xml @@ -9,17 +9,15 @@ - - - - + - + + @@ -31,8 +29,6 @@ - - @@ -48,11 +44,11 @@ - - + + @@ -64,8 +60,6 @@ - - @@ -80,11 +74,11 @@ - - + + @@ -96,8 +90,6 @@ - - @@ -112,10 +104,11 @@ - + + @@ -127,8 +120,6 @@ - - @@ -143,10 +134,11 @@ - + + @@ -158,8 +150,6 @@ - - @@ -174,10 +164,11 @@ - + + @@ -189,8 +180,6 @@ - - From b3f41263d4300574a0670f2d760c6862b126a7dc Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Thu, 24 Oct 2024 13:48:25 +0900 Subject: [PATCH 21/38] feat(aip_launcher): set multi fov for mirror and body pointcloud reduction (#335) * folder for point_filters config yaml * set front_lower multiFOV * set rear_lower multiFOV * set front_upper multiFOV * fix front_upper FOV * fix rear_upper FOV * fix left_lower FOV * edit left_lower FOV * fix right_lower FOV * chore right_lower filter param to avoid rviz error Signed-off-by: kotaro-hihara * fix left_upper FOV Signed-off-by: kotaro-hihara * fix right_upper FOV * update left_upper FOV * update right_lower FOV * update rear_upper FOV * change defalut point_filters path * update FOV * update rear_upper FOV * update config of OT128(**_upper) to use one third --------- Signed-off-by: kotaro-hihara --- .../dafault}/point_filters_full.param.yaml | 0 .../point_filters_left_upper.param.yaml | 0 .../point_filters_one_half.param.yaml | 0 .../point_filters_one_third.param.yaml | 0 .../point_filters_right_upper.param.yaml | 0 .../point_filters_two_third.param.yaml | 0 .../front_lower/point_filters_full.param.yaml | 41 ++++ .../point_filters_one_third.param.yaml | 126 +++++++++++ .../point_filters_two_third.param.yaml | 84 ++++++++ .../front_upper/point_filters_full.param.yaml | 72 +++++++ .../point_filters_one_third.param.yaml | 157 ++++++++++++++ .../point_filters_two_third.param.yaml | 115 ++++++++++ .../left_lower/point_filters_full.param.yaml | 59 +++++ .../point_filters_one_third.param.yaml | 144 +++++++++++++ .../point_filters_two_third.param.yaml | 102 +++++++++ .../left_upper/point_filters_full.param.yaml | 72 +++++++ .../point_filters_one_third.param.yaml | 157 ++++++++++++++ .../point_filters_two_third.param.yaml | 115 ++++++++++ .../rear_lower/point_filters_full.param.yaml | 55 +++++ .../point_filters_one_third.param.yaml | 140 ++++++++++++ .../point_filters_two_third.param.yaml | 98 +++++++++ .../rear_upper/point_filters_full.param.yaml | 113 ++++++++++ .../point_filters_one_third.param.yaml | 198 +++++++++++++++++ .../point_filters_two_third.param.yaml | 156 ++++++++++++++ .../right_lower/point_filters_full.param.yaml | 60 ++++++ .../point_filters_one_third.param.yaml | 145 +++++++++++++ .../point_filters_two_third.param.yaml | 103 +++++++++ .../right_upper/point_filters_full.param.yaml | 116 ++++++++++ .../point_filters_one_third.param.yaml | 201 ++++++++++++++++++ .../point_filters_two_third.param.yaml | 159 ++++++++++++++ .../launch/hesai_OT128.launch.xml | 2 +- .../launch/hesai_QT128.launch.xml | 2 +- aip_x2_gen2_launch/launch/lidar.launch.xml | 18 +- 33 files changed, 2801 insertions(+), 9 deletions(-) rename aip_x2_gen2_launch/config/{ => point_filters/dafault}/point_filters_full.param.yaml (100%) rename aip_x2_gen2_launch/config/{ => point_filters/dafault}/point_filters_left_upper.param.yaml (100%) rename aip_x2_gen2_launch/config/{ => point_filters/dafault}/point_filters_one_half.param.yaml (100%) rename aip_x2_gen2_launch/config/{ => point_filters/dafault}/point_filters_one_third.param.yaml (100%) rename aip_x2_gen2_launch/config/{ => point_filters/dafault}/point_filters_right_upper.param.yaml (100%) rename aip_x2_gen2_launch/config/{ => point_filters/dafault}/point_filters_two_third.param.yaml (100%) create mode 100644 aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml create mode 100644 aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_full.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters_full.param.yaml rename to aip_x2_gen2_launch/config/point_filters/dafault/point_filters_full.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters_left_upper.param.yaml b/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_left_upper.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters_left_upper.param.yaml rename to aip_x2_gen2_launch/config/point_filters/dafault/point_filters_left_upper.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters_one_half.param.yaml b/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_one_half.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters_one_half.param.yaml rename to aip_x2_gen2_launch/config/point_filters/dafault/point_filters_one_half.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_one_third.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters_one_third.param.yaml rename to aip_x2_gen2_launch/config/point_filters/dafault/point_filters_one_third.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters_right_upper.param.yaml b/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_right_upper.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters_right_upper.param.yaml rename to aip_x2_gen2_launch/config/point_filters/dafault/point_filters_right_upper.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_two_third.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters_two_third.param.yaml rename to aip_x2_gen2_launch/config/point_filters/dafault/point_filters_two_third.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml new file mode 100644 index 00000000..33d53465 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml @@ -0,0 +1,41 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,128], [0, 232,300], + [1, 60,124], [1, 236,300], + [2, 60,122], [2, 238,300], + [3, 60,120], [3, 240,300], + [4, 60,118], [4, 242,300], + [5, 60,118], [5, 242,300], + [6, 60,114], [6, 246,300], + [7, 60,114], [7, 246,300], + [8, 60,112], [8, 248,300], + [9, 60,112], [9, 248,300], + [10, 60,110], [10, 250,300], + [11, 60,110], [11, 250,300], + [12, 60,108], [12, 252,300], + [13, 60,108], [13, 252,300], + [14, 60,108], [14, 252,300], + [15, 60,108], [15, 252,300], + [16, 60,108], [16, 252,300], + [17, 60,108], [17, 252,300], + [18, 60,106], [18, 254,300], + [19, 60,106], [19, 254,300], + [20, 60,102], [20, 258,300], + [21, 60,102], [21, 258,300], + [22, 60,100], [22, 260,300], + [23, 60,100], [23, 260,300], + [24, 60,100], [24, 260,300], + [25, 60,100], [25, 260,300], + [26, 60,100], [26, 260,300], + [27, 60,100], [27, 260,300], + [28, 60,100], [28, 260,300], + [29, 60,100], [29, 260,300], + [30, 60,94], [30, 266,300], + [31, 60,94], [31, 266,300], + [32, 60,94], [32, 266,300], + [33, 60,94], [33, 266,300] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml new file mode 100644 index 00000000..71ba0993 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml @@ -0,0 +1,126 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,128], [0, 232,300], + [1, 60,124], [1, 236,300], + [2, 60,122], [2, 238,300], + [3, 60,120], [3, 240,300], + [4, 60,118], [4, 242,300], + [5, 60,118], [5, 242,300], + [6, 60,114], [6, 246,300], + [7, 60,114], [7, 246,300], + [8, 60,112], [8, 248,300], + [9, 60,112], [9, 248,300], + [10, 60,110], [10, 250,300], + [11, 60,110], [11, 250,300], + [12, 60,108], [12, 252,300], + [13, 60,108], [13, 252,300], + [14, 60,108], [14, 252,300], + [15, 60,108], [15, 252,300], + [16, 60,108], [16, 252,300], + [17, 60,108], [17, 252,300], + [18, 60,106], [18, 254,300], + [19, 60,106], [19, 254,300], + [20, 60,102], [20, 258,300], + [21, 60,102], [21, 258,300], + [22, 60,100], [22, 260,300], + [23, 60,100], [23, 260,300], + [24, 60,100], [24, 260,300], + [25, 60,100], [25, 260,300], + [26, 60,100], [26, 260,300], + [27, 60,100], [27, 260,300], + [28, 60,100], [28, 260,300], + [29, 60,100], [29, 260,300], + [30, 60,94], [30, 266,300], + [31, 60,94], [31, 266,300], + [32, 60,94], [32, 266,300], + [33, 60,94], [33, 266,300], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml new file mode 100644 index 00000000..0ccc1830 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml @@ -0,0 +1,84 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,128], [0, 232,300], + [1, 60,124], [1, 236,300], + [2, 60,122], [2, 238,300], + [3, 60,120], [3, 240,300], + [4, 60,118], [4, 242,300], + [5, 60,118], [5, 242,300], + [6, 60,114], [6, 246,300], + [7, 60,114], [7, 246,300], + [8, 60,112], [8, 248,300], + [9, 60,112], [9, 248,300], + [10, 60,110], [10, 250,300], + [11, 60,110], [11, 250,300], + [12, 60,108], [12, 252,300], + [13, 60,108], [13, 252,300], + [14, 60,108], [14, 252,300], + [15, 60,108], [15, 252,300], + [16, 60,108], [16, 252,300], + [17, 60,108], [17, 252,300], + [18, 60,106], [18, 254,300], + [19, 60,106], [19, 254,300], + [20, 60,102], [20, 258,300], + [21, 60,102], [21, 258,300], + [22, 60,100], [22, 260,300], + [23, 60,100], [23, 260,300], + [24, 60,100], [24, 260,300], + [25, 60,100], [25, 260,300], + [26, 60,100], [26, 260,300], + [27, 60,100], [27, 260,300], + [28, 60,100], [28, 260,300], + [29, 60,100], [29, 260,300], + [30, 60,94], [30, 266,300], + [31, 60,94], [31, 266,300], + [32, 60,94], [32, 266,300], + [33, 60,94], [33, 266,300], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml new file mode 100644 index 00000000..86c11c7f --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml @@ -0,0 +1,72 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60, 93],[0, 266, 300], + [1, 60, 93],[1, 266, 300], + [2, 60, 93],[2, 266, 300], + [3, 60, 93],[3, 266, 300], + [4, 268, 300], + [5, 268, 300], + [6, 268, 300], + [7, 268, 300], + [8, 268, 300], + [10, 272, 300], + [11, 272, 300], + [12, 272, 300], + [13, 272, 300], + [14, 266, 300], + [15, 266, 300], + [16, 266, 300], + [18, 263, 300], + [19, 263, 300], + [20, 263, 300], + [22, 270, 300], + [23, 270, 300], + [24, 270, 300], + [30, 267, 300], + [31, 267, 300], + [32, 267, 300], + [38, 272, 300], + [45, 60, 93],[45, 267, 300], + [52, 195, 300], + [58, 261, 300], + [59, 261, 300], + [60, 261, 300], + [72, 269, 300], + [73, 269, 300], + [74, 269, 300], + [88, 273, 300], + [89, 273, 300], + [90, 273, 300], + [91, 60, 93],[91, 273, 300], + [92, 60, 93],[92, 268, 300], + [93, 60, 93],[93, 273, 300], + [94, 60, 93],[94, 273, 300], + [95, 60, 93],[95, 273, 300], + [96, 60, 93],[96, 273, 300], + [97, 60, 93],[97, 273, 300], + [98, 60, 93],[98, 273, 300], + [99, 60, 93],[99, 273, 300], + [100, 60, 93.5],[100, 273, 300], + [101, 273, 300], + [102, 273, 300], + [103, 274, 300], + [104, 274, 300], + [105, 275, 300], + [106, 275, 300], + [107, 275, 300], + [108, 275, 300], + [112, 60, 93], + [116, 60, 93], + [120, 60, 93],[120, 265, 300], + [121, 60, 93],[121, 265, 300], + [122, 60, 93],[122, 265, 300], + [123, 60, 94],[123, 265, 300], + [124, 60, 94],[124, 265, 300], + [125, 60, 97],[125, 256, 300], + [126, 60, 97],[126, 256, 300], + [127, 60, 97],[127, 256, 300] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml new file mode 100644 index 00000000..a61e40e8 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml @@ -0,0 +1,157 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60, 93],[0, 266, 300], + [1, 60, 93],[1, 266, 300], + [2, 60, 93],[2, 266, 300], + [3, 60, 93],[3, 266, 300], + [4, 268, 300], + [5, 268, 300], + [6, 268, 300], + [7, 268, 300], + [8, 268, 300], + [10, 272, 300], + [11, 272, 300], + [12, 272, 300], + [13, 272, 300], + [14, 266, 300], + [15, 266, 300], + [16, 266, 300], + [18, 263, 300], + [19, 263, 300], + [20, 263, 300], + [22, 270, 300], + [23, 270, 300], + [24, 270, 300], + [30, 267, 300], + [31, 267, 300], + [32, 267, 300], + [38, 272, 300], + [45, 60, 93],[45, 267, 300], + [52, 195, 300], + [58, 261, 300], + [59, 261, 300], + [60, 261, 300], + [72, 269, 300], + [73, 269, 300], + [74, 269, 300], + [88, 273, 300], + [89, 273, 300], + [90, 273, 300], + [91, 60, 93],[91, 273, 300], + [92, 60, 93],[92, 268, 300], + [93, 60, 93],[93, 273, 300], + [94, 60, 93],[94, 273, 300], + [95, 60, 93],[95, 273, 300], + [96, 60, 93],[96, 273, 300], + [97, 60, 93],[97, 273, 300], + [98, 60, 93],[98, 273, 300], + [99, 60, 93],[99, 273, 300], + [100, 60, 93.5],[100, 273, 300], + [101, 273, 300], + [102, 273, 300], + [103, 274, 300], + [104, 274, 300], + [105, 275, 300], + [106, 275, 300], + [107, 275, 300], + [108, 275, 300], + [112, 60, 93], + [116, 60, 93], + [120, 60, 93],[120, 265, 300], + [121, 60, 93],[121, 265, 300], + [122, 60, 93],[122, 265, 300], + [123, 60, 94],[123, 265, 300], + [124, 60, 94],[124, 265, 300], + [125, 60, 97],[125, 256, 300], + [126, 60, 97],[126, 256, 300], + [127, 60, 97],[127, 256, 300], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml new file mode 100644 index 00000000..ee82ff19 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml @@ -0,0 +1,115 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60, 93],[0, 266, 300], + [1, 60, 93],[1, 266, 300], + [2, 60, 93],[2, 266, 300], + [3, 60, 93],[3, 266, 300], + [4, 268, 300], + [5, 268, 300], + [6, 268, 300], + [7, 268, 300], + [8, 268, 300], + [10, 272, 300], + [11, 272, 300], + [12, 272, 300], + [13, 272, 300], + [14, 266, 300], + [15, 266, 300], + [16, 266, 300], + [18, 263, 300], + [19, 263, 300], + [20, 263, 300], + [22, 270, 300], + [23, 270, 300], + [24, 270, 300], + [30, 267, 300], + [31, 267, 300], + [32, 267, 300], + [38, 272, 300], + [45, 60, 93],[45, 267, 300], + [52, 195, 300], + [58, 261, 300], + [59, 261, 300], + [60, 261, 300], + [72, 269, 300], + [73, 269, 300], + [74, 269, 300], + [88, 273, 300], + [89, 273, 300], + [90, 273, 300], + [91, 60, 93],[91, 273, 300], + [92, 60, 93],[92, 268, 300], + [93, 60, 93],[93, 273, 300], + [94, 60, 93],[94, 273, 300], + [95, 60, 93],[95, 273, 300], + [96, 60, 93],[96, 273, 300], + [97, 60, 93],[97, 273, 300], + [98, 60, 93],[98, 273, 300], + [99, 60, 93],[99, 273, 300], + [100, 60, 93.5],[100, 273, 300], + [101, 273, 300], + [102, 273, 300], + [103, 274, 300], + [104, 274, 300], + [105, 275, 300], + [106, 275, 300], + [107, 275, 300], + [108, 275, 300], + [112, 60, 93], + [116, 60, 93], + [120, 60, 93],[120, 265, 300], + [121, 60, 93],[121, 265, 300], + [122, 60, 93],[122, 265, 300], + [123, 60, 94],[123, 265, 300], + [124, 60, 94],[124, 265, 300], + [125, 60, 97],[125, 256, 300], + [126, 60, 97],[126, 256, 300], + [127, 60, 97],[127, 256, 300], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml new file mode 100644 index 00000000..2a2b0a5b --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml @@ -0,0 +1,59 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 360], + [3, 0, 170],[3, 199.5, 300], + [4, 0, 155],[4, 203, 300], + [5, 0, 152],[5, 206, 217],[5, 220, 300], + [6, 0, 146],[6, 217, 300], + [7, 0, 143],[7, 235, 300], + [8, 0, 143],[8, 228.5, 300], + [9, 0, 142],[9, 228.5, 300], + [10, 0, 141],[10, 235, 300], + [11, 0, 140],[11, 235, 300], + [12, 0, 140],[12, 235, 300], + [13, 0, 135],[13, 235, 300], + [14, 0, 133],[14, 235, 300], + [15, 0, 131],[15, 235, 300], + [16, 0, 129],[16, 235, 300], + [17, 0, 127],[17, 235, 300], + [18, 0, 127],[18, 235, 300], + [19, 0, 126],[19, 235, 300], + [20, 0, 125],[20, 235, 300], + [21, 0, 125],[21, 235, 300], + [22, 0, 124],[22, 235, 300], + [23, 0, 124],[23, 235, 300], + [24, 0, 122],[24, 235, 300], + [25, 0, 122],[25, 235, 300], + [26, 0, 121],[26, 235, 300], + [27, 0, 120],[27, 235, 300], + [28, 0, 119],[28, 242, 300], + [29, 0, 118],[29, 242, 300], + [30, 0, 110],[30, 242, 300], + [31, 0, 109],[31, 244, 265], + [32, 0, 109],[32, 244, 265], + [33, 0, 108],[33, 244, 265], + [34, 0, 108],[34, 244, 265], + [35, 0, 107],[35, 244, 265], + [36, 0, 107],[36, 244, 265], + [37, 0, 106],[37, 244, 265], + [38, 0, 106],[38, 244, 265], + [39, 0, 105],[39, 244, 265], + [40, 0, 105],[40, 244, 265], + [41, 0, 104],[41, 244, 265], + [42, 0, 104],[42, 244, 265], + [43, 0, 103],[43, 244, 265], + [44, 0, 102], + [45, 0, 101], + [46, 0, 96], + [47, 0, 95], + [48, 0, 95], + [49, 0, 94], + [50, 0, 93], + [51, 0, 93] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml new file mode 100644 index 00000000..d05dd26f --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml @@ -0,0 +1,144 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 170],[2, 192, 300], + [3, 0, 160],[3, 199.5, 300], + [4, 0, 155],[4, 203, 300], + [5, 0, 152],[5, 206, 217],[5, 220, 300], + [6, 0, 146],[6, 217, 300], + [7, 0, 143],[7, 235, 300], + [8, 0, 139],[8, 228.5, 300], + [9, 0, 135],[9, 228.5, 300], + [10, 0, 133],[10, 235, 300], + [11, 0, 132],[11, 235, 300], + [12, 0, 131],[12, 235, 300], + [13, 0, 129],[13, 235, 300], + [14, 0, 127],[14, 235, 300], + [15, 0, 126],[15, 235, 300], + [16, 0, 124],[16, 235, 300], + [17, 0, 122],[17, 235, 300], + [18, 0, 121],[18, 235, 300], + [19, 0, 120],[19, 235, 300], + [20, 0, 119],[20, 235, 300], + [21, 0, 118],[21, 235, 300], + [22, 0, 117],[22, 235, 300], + [23, 0, 116],[23, 235, 300], + [24, 0, 115],[24, 235, 300], + [25, 0, 114],[25, 235, 300], + [26, 0, 113],[26, 235, 300], + [27, 0, 112],[27, 235, 300], + [28, 0, 111],[28, 242, 300], + [29, 0, 110],[29, 242, 300], + [30, 0, 110],[30, 242, 300], + [31, 0, 109],[31, 244, 265], + [32, 0, 109],[32, 244, 265], + [33, 0, 108],[33, 244, 265], + [34, 0, 108],[34, 244, 265], + [35, 0, 107],[35, 244, 265], + [36, 0, 107],[36, 244, 265], + [37, 0, 106],[37, 244, 265], + [38, 0, 106],[38, 244, 265], + [39, 0, 105],[39, 244, 265], + [40, 0, 105],[40, 244, 265], + [41, 0, 104],[41, 244, 265], + [42, 0, 104],[42, 244, 265], + [43, 0, 103],[43, 244, 265], + [44, 0, 102], + [45, 0, 101], + [46, 0, 96], + [47, 0, 95], + [48, 0, 95], + [49, 0, 94], + [50, 0, 93], + [51, 0, 93], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml new file mode 100644 index 00000000..f7445d31 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml @@ -0,0 +1,102 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 170],[2, 192, 300], + [3, 0, 160],[3, 199.5, 300], + [4, 0, 155],[4, 203, 300], + [5, 0, 152],[5, 206, 217],[5, 220, 300], + [6, 0, 146],[6, 217, 300], + [7, 0, 143],[7, 235, 300], + [8, 0, 139],[8, 228.5, 300], + [9, 0, 135],[9, 228.5, 300], + [10, 0, 133],[10, 235, 300], + [11, 0, 132],[11, 235, 300], + [12, 0, 131],[12, 235, 300], + [13, 0, 129],[13, 235, 300], + [14, 0, 127],[14, 235, 300], + [15, 0, 126],[15, 235, 300], + [16, 0, 124],[16, 235, 300], + [17, 0, 122],[17, 235, 300], + [18, 0, 121],[18, 235, 300], + [19, 0, 120],[19, 235, 300], + [20, 0, 119],[20, 235, 300], + [21, 0, 118],[21, 235, 300], + [22, 0, 117],[22, 235, 300], + [23, 0, 116],[23, 235, 300], + [24, 0, 115],[24, 235, 300], + [25, 0, 114],[25, 235, 300], + [26, 0, 113],[26, 235, 300], + [27, 0, 112],[27, 235, 300], + [28, 0, 111],[28, 242, 300], + [29, 0, 110],[29, 242, 300], + [30, 0, 110],[30, 242, 300], + [31, 0, 109],[31, 244, 265], + [32, 0, 109],[32, 244, 265], + [33, 0, 108],[33, 244, 265], + [34, 0, 108],[34, 244, 265], + [35, 0, 107],[35, 244, 265], + [36, 0, 107],[36, 244, 265], + [37, 0, 106],[37, 244, 265], + [38, 0, 106],[38, 244, 265], + [39, 0, 105],[39, 244, 265], + [40, 0, 105],[40, 244, 265], + [41, 0, 104],[41, 244, 265], + [42, 0, 104],[42, 244, 265], + [43, 0, 103],[43, 244, 265], + [44, 0, 102], + [45, 0, 101], + [46, 0, 96], + [47, 0, 95], + [48, 0, 95], + [49, 0, 94], + [50, 0, 93], + [51, 0, 93], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml new file mode 100644 index 00000000..366958ed --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml @@ -0,0 +1,72 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [11, 322, 336],[11, 354, 360], + [15, 143.95, 147.1],[15, 318, 360], + [19, 330, 360], + [23, 330, 360], + [31, 348, 360], + [38, 350, 360], + [45, 339, 360], + [52, 339, 360], + [59, 99, 101.5],[59, 330, 360], + [60, 330, 341], + [61, 330, 341], + [62, 330, 341], + [63, 330, 341], + [64, 330, 341], + [65, 330, 341], + [66, 319, 360], + [67, 330, 342], + [68, 330, 342], + [69, 330, 342], + [70, 317, 360], + [71, 331, 341], + [72, 319, 322],[72, 331, 341], + [73, 316, 360], + [74, 318, 341], + [75, 318, 341], + [76, 318, 341], + [77, 318, 360], + [77, 318, 360], + [78, 318, 360], + [79, 315, 360], + [80, 318, 360], + [81, 318, 360], + [82, 318, 360], + [83, 318, 360], + [84, 319, 360], + [85, 319, 360], + [86, 319, 360], + [87, 320, 360], + [88, 315, 360], + [89, 322, 323], + [91, 287.55, 289.90], + [92, 287.55, 289.75],[92, 315, 360], + [96, 316, 360], + [100, 316, 360], + [104, 316, 360], + [108, 314, 360], + [109, 314.74, 60], + [110, 312.74, 360], + [111, 312.73, 360], + [112, 308.48, 360], + [113, 305.68, 360], + [114, 308.58, 360], + [115, 307, 360], + [116, 303, 360], + [117, 303, 360], + [118, 303.81, 360], + [119, 302.09, 360], + [120, 298.42, 360], + [121, 299.63, 360], + [122, 298.42, 360], + [123, 298.42, 360], + [124, 293, 360], + [125, 90, 91],[125, 293, 360], + [126, 90, 91],[126, 292.12, 360], + [127, 90, 91],[127, 289.71, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml new file mode 100644 index 00000000..d97f176e --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml @@ -0,0 +1,157 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [11, 322, 336],[11, 354, 360], + [15, 143.95, 147.1],[15, 318, 360], + [19, 330, 360], + [23, 330, 360], + [31, 348, 360], + [38, 350, 360], + [45, 339, 360], + [52, 339, 360], + [59, 99, 101.5],[59, 330, 360], + [60, 330, 341], + [61, 330, 341], + [62, 330, 341], + [63, 330, 341], + [64, 330, 341], + [65, 330, 341], + [66, 319, 360], + [67, 330, 342], + [68, 330, 342], + [69, 330, 342], + [70, 317, 360], + [71, 331, 341], + [72, 319, 322],[72, 331, 341], + [73, 316, 360], + [74, 318, 341], + [75, 318, 341], + [76, 318, 341], + [77, 318, 360], + [77, 318, 360], + [78, 318, 360], + [79, 315, 360], + [80, 318, 360], + [81, 318, 360], + [82, 318, 360], + [83, 318, 360], + [84, 319, 360], + [85, 319, 360], + [86, 319, 360], + [87, 320, 360], + [88, 315, 360], + [89, 322, 323], + [91, 287.55, 289.90], + [92, 287.55, 289.75],[92, 315, 360], + [96, 316, 360], + [100, 316, 360], + [104, 316, 360], + [108, 314, 360], + [109, 314.74, 60], + [110, 312.74, 360], + [111, 312.73, 360], + [112, 308.48, 360], + [113, 305.68, 360], + [114, 308.58, 360], + [115, 307, 360], + [116, 303, 360], + [117, 303, 360], + [118, 303.81, 360], + [119, 302.09, 360], + [120, 298.42, 360], + [121, 299.63, 360], + [122, 298.42, 360], + [123, 298.42, 360], + [124, 293, 360], + [125, 90, 91],[125, 293, 360], + [126, 90, 91],[126, 292.12, 360], + [127, 90, 91],[127, 289.71, 360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml new file mode 100644 index 00000000..18753f9a --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml @@ -0,0 +1,115 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [11, 322, 336],[11, 354, 360], + [15, 143.95, 147.1],[15, 318, 360], + [19, 330, 360], + [23, 330, 360], + [31, 348, 360], + [38, 350, 360], + [45, 339, 360], + [52, 339, 360], + [59, 99, 101.5],[59, 330, 360], + [60, 330, 341], + [61, 330, 341], + [62, 330, 341], + [63, 330, 341], + [64, 330, 341], + [65, 330, 341], + [66, 319, 360], + [67, 330, 342], + [68, 330, 342], + [69, 330, 342], + [70, 317, 360], + [71, 331, 341], + [72, 319, 322],[72, 331, 341], + [73, 316, 360], + [74, 318, 341], + [75, 318, 341], + [76, 318, 341], + [77, 318, 360], + [77, 318, 360], + [78, 318, 360], + [79, 315, 360], + [80, 318, 360], + [81, 318, 360], + [82, 318, 360], + [83, 318, 360], + [84, 319, 360], + [85, 319, 360], + [86, 319, 360], + [87, 320, 360], + [88, 315, 360], + [89, 322, 323], + [91, 287.55, 289.90], + [92, 287.55, 289.75],[92, 315, 360], + [96, 316, 360], + [100, 316, 360], + [104, 316, 360], + [108, 314, 360], + [109, 314.74, 60], + [110, 312.74, 360], + [111, 312.73, 360], + [112, 308.48, 360], + [113, 305.68, 360], + [114, 308.58, 360], + [115, 307, 360], + [116, 303, 360], + [117, 303, 360], + [118, 303.81, 360], + [119, 302.09, 360], + [120, 298.42, 360], + [121, 299.63, 360], + [122, 298.42, 360], + [123, 298.42, 360], + [124, 293, 360], + [125, 90, 91],[125, 293, 360], + [126, 90, 91],[126, 292.12, 360], + [127, 90, 91],[127, 289.71, 360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml new file mode 100644 index 00000000..770a6c07 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,148], [0, 212,300], + [1, 60,144], [1, 216,300], + [2, 60,142], [2, 218,300], + [3, 60,140], [3, 220,300], + [4, 60,138], [4, 222,300], + [5, 60,138], [5, 222,300], + [6, 60,134], [6, 226,300], + [7, 60,134], [7, 226,300], + [8, 60,132], [8, 228,300], + [9, 60,132], [9, 228,300], + [10, 60,130], [10, 230,300], + [11, 60,120], [11, 240,300], + [12, 60,118], [12, 242,300], + [13, 60,118], [13, 242,300], + [14, 60,118], [14, 242,300], + [15, 60,118], [15, 242,300], + [16, 60,118], [16, 242,300], + [17, 60,118], [17, 242,300], + [18, 60,116], [18, 244,300], + [19, 60,116], [19, 244,300], + [20, 60,112], [20, 248,300], + [21, 60,112], [21, 248,300], + [22, 60,110], [22, 250,300], + [23, 60,110], [23, 250,300], + [24, 60,110], [24, 250,300], + [25, 60,110], [25, 250,300], + [26, 60,110], [26, 250,300], + [27, 60,106], [27, 254,300], + [28, 60,106], [28, 254,300], + [29, 60,106], [29, 254,300], + [30, 60,104], [30, 256,300], + [31, 60,104], [31, 256,300], + [32, 60,104], [32, 256,300], + [33, 60,104], [33, 256,300], + [34, 60,104], [34, 256,300], + [35, 60,100], [35, 260,300], + [36, 60,100], [36, 260,300], + [37, 60,100], [37, 260,300], + [38, 60,100], [38, 260,300], + [39, 60,96], [39, 264,300], + [40, 60,96], [40, 264,300], + [41, 60,96], [41, 264,300], + [42, 60,96], [42, 264,300], + [43, 60,96], [43, 264,300], + [44, 60,94], [44, 266,300], + [45, 60,94], [45, 266,300], + [46, 60,94], [46, 266,300], + [47, 60,94], [47, 266,300] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml new file mode 100644 index 00000000..cc034395 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml @@ -0,0 +1,140 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,148], [0, 212,300], + [1, 60,144], [1, 216,300], + [2, 60,142], [2, 218,300], + [3, 60,140], [3, 220,300], + [4, 60,138], [4, 222,300], + [5, 60,138], [5, 222,300], + [6, 60,134], [6, 226,300], + [7, 60,134], [7, 226,300], + [8, 60,132], [8, 228,300], + [9, 60,132], [9, 228,300], + [10, 60,130], [10, 230,300], + [11, 60,120], [11, 240,300], + [12, 60,118], [12, 242,300], + [13, 60,118], [13, 242,300], + [14, 60,118], [14, 242,300], + [15, 60,118], [15, 242,300], + [16, 60,118], [16, 242,300], + [17, 60,118], [17, 242,300], + [18, 60,116], [18, 244,300], + [19, 60,116], [19, 244,300], + [20, 60,112], [20, 248,300], + [21, 60,112], [21, 248,300], + [22, 60,110], [22, 250,300], + [23, 60,110], [23, 250,300], + [24, 60,110], [24, 250,300], + [25, 60,110], [25, 250,300], + [26, 60,110], [26, 250,300], + [27, 60,106], [27, 254,300], + [28, 60,106], [28, 254,300], + [29, 60,106], [29, 254,300], + [30, 60,104], [30, 256,300], + [31, 60,104], [31, 256,300], + [32, 60,104], [32, 256,300], + [33, 60,104], [33, 256,300], + [34, 60,104], [34, 256,300], + [35, 60,100], [35, 260,300], + [36, 60,100], [36, 260,300], + [37, 60,100], [37, 260,300], + [38, 60,100], [38, 260,300], + [39, 60,96], [39, 264,300], + [40, 60,96], [40, 264,300], + [41, 60,96], [41, 264,300], + [42, 60,96], [42, 264,300], + [43, 60,96], [43, 264,300], + [44, 60,94], [44, 266,300], + [45, 60,94], [45, 266,300], + [46, 60,94], [46, 266,300], + [47, 60,94], [47, 266,300], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml new file mode 100644 index 00000000..267e2258 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml @@ -0,0 +1,98 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,148], [0, 212,300], + [1, 60,144], [1, 216,300], + [2, 60,142], [2, 218,300], + [3, 60,140], [3, 220,300], + [4, 60,138], [4, 222,300], + [5, 60,138], [5, 222,300], + [6, 60,134], [6, 226,300], + [7, 60,134], [7, 226,300], + [8, 60,132], [8, 228,300], + [9, 60,132], [9, 228,300], + [10, 60,130], [10, 230,300], + [11, 60,120], [11, 240,300], + [12, 60,118], [12, 242,300], + [13, 60,118], [13, 242,300], + [14, 60,118], [14, 242,300], + [15, 60,118], [15, 242,300], + [16, 60,118], [16, 242,300], + [17, 60,118], [17, 242,300], + [18, 60,116], [18, 244,300], + [19, 60,116], [19, 244,300], + [20, 60,112], [20, 248,300], + [21, 60,112], [21, 248,300], + [22, 60,110], [22, 250,300], + [23, 60,110], [23, 250,300], + [24, 60,110], [24, 250,300], + [25, 60,110], [25, 250,300], + [26, 60,110], [26, 250,300], + [27, 60,106], [27, 254,300], + [28, 60,106], [28, 254,300], + [29, 60,106], [29, 254,300], + [30, 60,104], [30, 256,300], + [31, 60,104], [31, 256,300], + [32, 60,104], [32, 256,300], + [33, 60,104], [33, 256,300], + [34, 60,104], [34, 256,300], + [35, 60,100], [35, 260,300], + [36, 60,100], [36, 260,300], + [37, 60,100], [37, 260,300], + [38, 60,100], [38, 260,300], + [39, 60,96], [39, 264,300], + [40, 60,96], [40, 264,300], + [41, 60,96], [41, 264,300], + [42, 60,96], [42, 264,300], + [43, 60,96], [43, 264,300], + [44, 60,94], [44, 266,300], + [45, 60,94], [45, 266,300], + [46, 60,94], [46, 266,300], + [47, 60,94], [47, 266,300], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml new file mode 100644 index 00000000..adcd8182 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml @@ -0,0 +1,113 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,95],[0, 270,300], + [1, 60,95],[1, 270,300], + [2, 60,95],[2, 270,300], + [3, 60,95],[3, 270,300], + [4, 60,98],[4, 270,300], + [5, 60,98],[5, 270,300], + [6, 60,98], + [7, 60,98], + [8, 60,92], + [9, 60,92], + [10, 270,300], + [11, 270,300], + [12, 270,300], + [14, 265,300], + [15, 265,300], + [16, 265,300], + [17, 263,300], + [18, 263,300], + [19, 263,300], + [20, 263,300], + [21, 263,300], + [22, 264,300], + [23, 264,300], + [24, 264,300], + [30, 60,99],[30,264,300], + [31, 60,99],[31,264,300], + [32, 60,99],[32,264,300], + [33, 60,90], + [34, 60,90], + [35, 60,90], + [36, 60,94], + [37, 60,94], + [38, 60,94], + [39, 60,94], + [40, 60,94], + [41, 60,90], + [42, 60,90], + [43, 60,90], + [44, 60,100],[44, 262,300], + [45, 60,100],[45, 262,300], + [46, 60,100],[46, 262,300], + [51, 60,100], + [52, 60,100], + [53, 60,100], + [58, 60,99],[58, 269,300], + [59, 60,99],[59, 269,300], + [60, 60,99],[60, 269,300], + [65, 259.5,300], + [66, 259.5,300], + [67, 259.5,300], + [72, 265,300], + [73, 265,300], + [74, 265,300], + [75, 265,300], + [76, 265,300], + [77, 265,300], + [78, 60,98],[78, 262,300], + [79, 60,98],[79, 262,300], + [80, 60,98],[80, 262,300], + [81, 60,98],[81, 262,300], + [82, 60,98],[82, 262,300], + [83, 60,98],[83, 262,300], + [84, 60,98],[84, 262,300], + [85, 60,98],[85, 262,300], + [86, 60,98],[86, 262,300], + [87, 60,100],[87, 262,300], + [88, 60,100],[88, 262,300], + [89, 60,100],[89, 262,300], + [90, 262,300], + [91, 60,101], [91, 270,300], + [92, 60,101], [92, 270,300], + [93, 60,101], [93, 270,300], + [94, 60,102], [94, 270,300], + [95, 60,102],[95, 267,300], + [96, 60,102],[96, 267,300], + [97, 60,102],[97, 267,300], + [98, 60,95], + [99, 60,95], + [100, 60,95], + [101, 60,95], + [102, 60,95], + [103, 60,99], + [104, 60,97], + [105, 60,97], + [106, 60,97], + [107, 60,98], + [108, 60,98], + [109, 60,98], + [110, 60,93], + [111, 60,93],[111, 270,300], + [112, 60,93],[112, 270,300], + [113, 60,93],[113, 270,300], + [114, 60,98], + [115, 60,98], + [116, 60,98], + [117, 60,98], + [118, 60,100], + [119, 60,101],[119, 269,300], + [120, 60,101],[120, 269,300], + [121, 60,101],[121, 264,300], + [122, 60,101],[122, 264,300], + [123, 60,103], [123, 264,300], + [124, 60,103], [124, 258,300], + [125, 60,103], [125, 258,300], + [126, 60,103], [126, 258,300], + [127, 60,103], [127, 258,300] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml new file mode 100644 index 00000000..550c33a9 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml @@ -0,0 +1,198 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,95],[0, 270,300], + [1, 60,95],[1, 270,300], + [2, 60,95],[2, 270,300], + [3, 60,95],[3, 270,300], + [4, 60,98],[4, 270,300], + [5, 60,98],[5, 270,300], + [6, 60,98], + [7, 60,98], + [8, 60,92], + [9, 60,92], + [10, 270,300], + [11, 270,300], + [12, 270,300], + [14, 265,300], + [15, 265,300], + [16, 265,300], + [17, 263,300], + [18, 263,300], + [19, 263,300], + [20, 263,300], + [21, 263,300], + [22, 264,300], + [23, 264,300], + [24, 264,300], + [30, 60,99],[30,264,300], + [31, 60,99],[31,264,300], + [32, 60,99],[32,264,300], + [33, 60,90], + [34, 60,90], + [35, 60,90], + [36, 60,94], + [37, 60,94], + [38, 60,94], + [39, 60,94], + [40, 60,94], + [41, 60,90], + [42, 60,90], + [43, 60,90], + [44, 60,100],[44, 262,300], + [45, 60,100],[45, 262,300], + [46, 60,100],[46, 262,300], + [51, 60,100], + [52, 60,100], + [53, 60,100], + [58, 60,99],[58, 269,300], + [59, 60,99],[59, 269,300], + [60, 60,99],[60, 269,300], + [65, 259.5,300], + [66, 259.5,300], + [67, 259.5,300], + [72, 265,300], + [73, 265,300], + [74, 265,300], + [75, 265,300], + [76, 265,300], + [77, 265,300], + [78, 60,98],[78, 262,300], + [79, 60,98],[79, 262,300], + [80, 60,98],[80, 262,300], + [81, 60,98],[81, 262,300], + [82, 60,98],[82, 262,300], + [83, 60,98],[83, 262,300], + [84, 60,98],[84, 262,300], + [85, 60,98],[85, 262,300], + [86, 60,98],[86, 262,300], + [87, 60,100],[87, 262,300], + [88, 60,100],[88, 262,300], + [89, 60,100],[89, 262,300], + [90, 262,300], + [91, 60,101], [91, 270,300], + [92, 60,101], [92, 270,300], + [93, 60,101], [93, 270,300], + [94, 60,102], [94, 270,300], + [95, 60,102],[95, 267,300], + [96, 60,102],[96, 267,300], + [97, 60,102],[97, 267,300], + [98, 60,95], + [99, 60,95], + [100, 60,95], + [101, 60,95], + [102, 60,95], + [103, 60,99], + [104, 60,97], + [105, 60,97], + [106, 60,97], + [107, 60,98], + [108, 60,98], + [109, 60,98], + [110, 60,93], + [111, 60,93],[111, 270,300], + [112, 60,93],[112, 270,300], + [113, 60,93],[113, 270,300], + [114, 60,98], + [115, 60,98], + [116, 60,98], + [117, 60,98], + [118, 60,100], + [119, 60,101],[119, 269,300], + [120, 60,101],[120, 269,300], + [121, 60,101],[121, 264,300], + [122, 60,101],[122, 264,300], + [123, 60,103], [123, 264,300], + [124, 60,103], [124, 258,300], + [125, 60,103], [125, 258,300], + [126, 60,103], [126, 258,300], + [127, 60,103], [127, 258,300], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml new file mode 100644 index 00000000..e3f7d468 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml @@ -0,0 +1,156 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60,95],[0, 270,300], + [1, 60,95],[1, 270,300], + [2, 60,95],[2, 270,300], + [3, 60,95],[3, 270,300], + [4, 60,98],[4, 270,300], + [5, 60,98],[5, 270,300], + [6, 60,98], + [7, 60,98], + [8, 60,92], + [9, 60,92], + [10, 270,300], + [11, 270,300], + [12, 270,300], + [14, 265,300], + [15, 265,300], + [16, 265,300], + [17, 263,300], + [18, 263,300], + [19, 263,300], + [20, 263,300], + [21, 263,300], + [22, 264,300], + [23, 264,300], + [24, 264,300], + [30, 60,99],[30,264,300], + [31, 60,99],[31,264,300], + [32, 60,99],[32,264,300], + [33, 60,90], + [34, 60,90], + [35, 60,90], + [36, 60,94], + [37, 60,94], + [38, 60,94], + [39, 60,94], + [40, 60,94], + [41, 60,90], + [42, 60,90], + [43, 60,90], + [44, 60,100],[44, 262,300], + [45, 60,100],[45, 262,300], + [46, 60,100],[46, 262,300], + [51, 60,100], + [52, 60,100], + [53, 60,100], + [58, 60,99],[58, 269,300], + [59, 60,99],[59, 269,300], + [60, 60,99],[60, 269,300], + [65, 259.5,300], + [66, 259.5,300], + [67, 259.5,300], + [72, 265,300], + [73, 265,300], + [74, 265,300], + [75, 265,300], + [76, 265,300], + [77, 265,300], + [78, 60,98],[78, 262,300], + [79, 60,98],[79, 262,300], + [80, 60,98],[80, 262,300], + [81, 60,98],[81, 262,300], + [82, 60,98],[82, 262,300], + [83, 60,98],[83, 262,300], + [84, 60,98],[84, 262,300], + [85, 60,98],[85, 262,300], + [86, 60,98],[86, 262,300], + [87, 60,100],[87, 262,300], + [88, 60,100],[88, 262,300], + [89, 60,100],[89, 262,300], + [90, 262,300], + [91, 60,101], [91, 270,300], + [92, 60,101], [92, 270,300], + [93, 60,101], [93, 270,300], + [94, 60,102], [94, 270,300], + [95, 60,102],[95, 267,300], + [96, 60,102],[96, 267,300], + [97, 60,102],[97, 267,300], + [98, 60,95], + [99, 60,95], + [100, 60,95], + [101, 60,95], + [102, 60,95], + [103, 60,99], + [104, 60,97], + [105, 60,97], + [106, 60,97], + [107, 60,98], + [108, 60,98], + [109, 60,98], + [110, 60,93], + [111, 60,93],[111, 270,300], + [112, 60,93],[112, 270,300], + [113, 60,93],[113, 270,300], + [114, 60,98], + [115, 60,98], + [116, 60,98], + [117, 60,98], + [118, 60,100], + [119, 60,101],[119, 269,300], + [120, 60,101],[120, 269,300], + [121, 60,101],[121, 264,300], + [122, 60,101],[122, 264,300], + [123, 60,103], [123, 264,300], + [124, 60,103], [124, 258,300], + [125, 60,103], [125, 258,300], + [126, 60,103], [126, 258,300], + [127, 60,103], [127, 258,300], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml new file mode 100644 index 00000000..95e10b77 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml @@ -0,0 +1,60 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 360], + [3, 60, 165],[3, 190, 300], + [4, 60, 158],[4, 203, 300], + [5, 60, 157],[5, 205, 300], + [6, 60, 156],[6, 211, 300], + [7, 60, 154],[7, 214, 300], + [8, 60, 152],[8, 217, 300], + [9, 60, 152],[9, 220, 300], + [10, 60, 151],[10, 221, 300], + [11, 60, 151],[11, 226, 300], + [12, 60, 151],[12, 229, 300], + [13, 60, 151],[13, 231, 300], + [14, 60, 150],[14, 232, 300], + [15, 60, 150],[15, 233, 300], + [16, 60, 150],[16, 234, 300], + [17, 60, 140],[17, 235, 300], + [18, 60, 135],[18, 235, 300], + [19, 60, 126],[19, 236, 300], + [20, 102, 126],[20, 240, 300], + [21, 102, 125],[21, 241, 300], + [22, 105, 125],[22, 241, 300], + [23, 105, 124],[23, 244, 300], + [24, 105, 123],[24, 245, 300], + [25, 106, 122],[25, 246, 300], + [26, 107, 121],[26, 247, 300], + [27, 108, 120],[27, 248, 300], + [28, 109, 119],[28, 248, 300], + [29, 110, 118],[29, 250, 300], + [30, 111, 117],[30, 250, 300], + [31, 112.5, 116],[31, 251, 300], + [32, 251, 300], + [33, 252, 300], + [34, 252, 300], + [35, 253, 300], + [36, 253, 300], + [37, 254, 300], + [38, 254, 300], + [39, 255, 300], + [40, 255, 300], + [41, 256, 300], + [42, 256, 300], + [43, 257, 300], + [44, 258, 300], + [45, 259, 300], + [46, 264, 300], + [47, 265, 300], + [48, 265, 300], + [49, 266, 300], + [50, 267, 300], + [51, 267, 300], + [52, 268, 300] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml new file mode 100644 index 00000000..351c5363 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml @@ -0,0 +1,145 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 360], + [3, 60, 165],[3, 190, 300], + [4, 60, 158],[4, 203, 300], + [5, 60, 157],[5, 205, 300], + [6, 60, 156],[6, 211, 300], + [7, 60, 154],[7, 214, 300], + [8, 60, 152],[8, 217, 300], + [9, 60, 152],[9, 220, 300], + [10, 60, 151],[10, 221, 300], + [11, 60, 151],[11, 226, 300], + [12, 60, 151],[12, 229, 300], + [13, 60, 151],[13, 231, 300], + [14, 60, 150],[14, 232, 300], + [15, 60, 150],[15, 233, 300], + [16, 60, 150],[16, 234, 300], + [17, 60, 140],[17, 235, 300], + [18, 60, 135],[18, 235, 300], + [19, 60, 126],[19, 236, 300], + [20, 103, 126],[20, 240, 300], + [21, 105, 125],[21, 241, 300], + [22, 105, 125],[22, 241, 300], + [23, 105, 124],[23, 244, 300], + [24, 105, 123],[24, 245, 300], + [25, 106, 122],[25, 246, 300], + [26, 107, 121],[26, 247, 300], + [27, 108, 120],[27, 248, 300], + [28, 109, 119],[28, 248, 300], + [29, 110, 118],[29, 250, 300], + [30, 111, 117],[30, 250, 300], + [31, 112.5, 116],[31, 251, 300], + [32, 251, 300], + [33, 252, 300], + [34, 252, 300], + [35, 253, 300], + [36, 253, 300], + [37, 254, 300], + [38, 254, 300], + [39, 255, 300], + [40, 255, 300], + [41, 256, 300], + [42, 256, 300], + [43, 257, 300], + [44, 258, 300], + [45, 259, 300], + [46, 264, 300], + [47, 265, 300], + [48, 265, 300], + [49, 266, 300], + [50, 267, 300], + [51, 267, 300], + [52, 268, 300], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml new file mode 100644 index 00000000..6e82a756 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml @@ -0,0 +1,103 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 360], + [3, 60, 165],[3, 190, 300], + [4, 60, 158],[4, 203, 300], + [5, 60, 157],[5, 205, 300], + [6, 60, 156],[6, 211, 300], + [7, 60, 154],[7, 214, 300], + [8, 60, 152],[8, 217, 300], + [9, 60, 152],[9, 220, 300], + [10, 60, 151],[10, 221, 300], + [11, 60, 151],[11, 226, 300], + [12, 60, 151],[12, 229, 300], + [13, 60, 151],[13, 231, 300], + [14, 60, 150],[14, 232, 300], + [15, 60, 150],[15, 233, 300], + [16, 60, 150],[16, 234, 300], + [17, 60, 140],[17, 235, 300], + [18, 60, 135],[18, 235, 300], + [19, 60, 126],[19, 236, 300], + [20, 103, 126],[20, 240, 300], + [21, 105, 125],[21, 241, 300], + [22, 105, 125],[22, 241, 300], + [23, 105, 124],[23, 244, 300], + [24, 105, 123],[24, 245, 300], + [25, 106, 122],[25, 246, 300], + [26, 107, 121],[26, 247, 300], + [27, 108, 120],[27, 248, 300], + [28, 109, 119],[28, 248, 300], + [29, 110, 118],[29, 250, 300], + [30, 111, 117],[30, 250, 300], + [31, 112.5, 116],[31, 251, 300], + [32, 251, 300], + [33, 252, 300], + [34, 252, 300], + [35, 253, 300], + [36, 253, 300], + [37, 254, 300], + [38, 254, 300], + [39, 255, 300], + [40, 255, 300], + [41, 256, 300], + [42, 256, 300], + [43, 257, 300], + [44, 258, 300], + [45, 259, 300], + [46, 264, 300], + [47, 265, 300], + [48, 265, 300], + [49, 266, 300], + [50, 267, 300], + [51, 267, 300], + [52, 268, 300], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml new file mode 100644 index 00000000..a0fc854a --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml @@ -0,0 +1,116 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 15], + [1, 0, 15], + [2, 0, 15], + [3, 0, 15], + [23, 0, 10], + [24, 0, 10], + [25, 0, 10], + [26, 0, 10], + [27, 0, 10], + [28, 0, 10], + [29, 0, 10], + [30, 0, 15], + [31, 0, 15], + [32, 0, 15], + [33, 0, 10], + [34, 0, 10], + [35, 0, 10], + [36, 0, 10], + [37, 0, 10], + [38, 0, 10], + [39, 0, 10], + [40, 0, 10], + [41, 0, 10], + [42, 0, 10], + [43, 0, 10], + [44, 0, 10], + [45, 0, 25], + [46, 0, 25], + [47, 0, 25], + [48, 0, 26], + [49, 0, 26], + [50, 0, 27], + [51, 0, 40], + [52, 0, 40], + [53, 0, 40], + [54, 0, 40], + [55, 0, 40], + [56, 0, 40], + [57, 0, 45], + [58, 0, 45], + [59, 0, 45], + [60, 0, 45], + [61, 0, 40], + [62, 0, 35], + [63, 0, 35], + [64, 0, 35], + [65, 0, 35], + [66, 0, 40], + [67, 0, 40], + [68, 0, 42], + [69, 0, 42], + [70, 0, 42], + [71, 0, 44], + [72, 0, 44], + [73, 0, 44], + [74, 0, 44], + [75, 0, 44], + [76, 0, 45], + [77, 0, 45], + [78, 0, 45], + [79, 0, 45], + [80, 0, 45], + [81, 0, 45], + [82, 0, 45], + [83, 0, 45], + [84, 0, 45], + [85, 0, 45], + [86, 0, 45], + [87, 0, 45], + [88, 0, 45], + [89, 0, 45], + [90, 0, 45], + [91, 0, 45], + [92, 0, 45], + [93, 0, 44], + [94, 0, 44], + [95, 0, 44], + [96, 0, 44], + [97, 0, 44], + [98, 0, 44], + [99, 0, 44], + [100, 0, 46], + [101, 0, 46], + [102, 0, 46], + [103, 0, 44], + [104, 0, 44],[104, 269, 360], + [105, 0, 44],[105, 269, 360], + [106, 0, 44],[106, 269, 360], + [107, 0, 44],[107, 269, 360], + [108, 0, 46],[108, 269, 360], + [109, 0, 47],[109, 269, 360], + [110, 0, 48],[110, 87.78, 89.97],[110, 269, 360], + [111, 0, 49],[111, 269, 360], + [112, 0, 50],[112, 269, 360], + [113, 0, 52],[113, 269, 360], + [114, 0, 53],[114, 269, 360], + [115, 0, 54],[115, 269, 360], + [116, 0, 55],[116, 269, 360], + [117, 0, 56],[117, 269, 360], + [118, 0, 57],[118, 269, 360], + [119, 0, 58],[119, 269, 360], + [120, 0, 59],[120, 269, 360], + [121, 0, 61],[121, 269, 360], + [122, 0, 63],[122, 268, 360], + [123, 0, 66],[123, 268, 360], + [124, 0, 69],[124, 268, 360], + [125, 0, 69],[125, 267, 360], + [126, 0, 71],[126, 267, 360], + [127, 0, 72],[127, 267, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml new file mode 100644 index 00000000..35f6a53f --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml @@ -0,0 +1,201 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 15], + [1, 0, 15], + [2, 0, 15], + [3, 0, 15], + [23, 0, 10], + [24, 0, 10], + [25, 0, 10], + [26, 0, 10], + [27, 0, 10], + [28, 0, 10], + [29, 0, 10], + [30, 0, 15], + [31, 0, 15], + [32, 0, 15], + [33, 0, 10], + [34, 0, 10], + [35, 0, 10], + [36, 0, 10], + [37, 0, 10], + [38, 0, 10], + [39, 0, 10], + [40, 0, 10], + [41, 0, 10], + [42, 0, 10], + [43, 0, 10], + [44, 0, 10], + [45, 0, 25], + [46, 0, 25], + [47, 0, 25], + [48, 0, 26], + [49, 0, 26], + [50, 0, 27], + [51, 0, 40], + [52, 0, 40], + [53, 0, 40], + [54, 0, 40], + [55, 0, 40], + [56, 0, 40], + [57, 0, 45], + [58, 0, 45], + [59, 0, 45], + [60, 0, 45], + [61, 0, 40], + [62, 0, 35], + [63, 0, 35], + [64, 0, 35], + [65, 0, 35], + [66, 0, 40], + [67, 0, 40], + [68, 0, 42], + [69, 0, 42], + [70, 0, 42], + [71, 0, 44], + [72, 0, 44], + [73, 0, 44], + [74, 0, 44], + [75, 0, 44], + [76, 0, 45], + [77, 0, 45], + [78, 0, 45], + [79, 0, 45], + [80, 0, 45], + [81, 0, 45], + [82, 0, 45], + [83, 0, 45], + [84, 0, 45], + [85, 0, 45], + [86, 0, 45], + [87, 0, 45], + [88, 0, 45], + [89, 0, 45], + [90, 0, 45], + [91, 0, 45], + [92, 0, 45], + [93, 0, 44], + [94, 0, 44], + [95, 0, 44], + [96, 0, 44], + [97, 0, 44], + [98, 0, 44], + [99, 0, 44], + [100, 0, 46], + [101, 0, 46], + [102, 0, 46], + [103, 0, 44], + [104, 0, 44],[104, 269, 360], + [105, 0, 44],[105, 269, 360], + [106, 0, 44],[106, 269, 360], + [107, 0, 44],[107, 269, 360], + [108, 0, 46],[108, 269, 360], + [109, 0, 47],[109, 269, 360], + [110, 0, 48],[110, 87.78, 89.97],[110, 269, 360], + [111, 0, 49],[111, 269, 360], + [112, 0, 50],[112, 269, 360], + [113, 0, 52],[113, 269, 360], + [114, 0, 53],[114, 269, 360], + [115, 0, 54],[115, 269, 360], + [116, 0, 55],[116, 269, 360], + [117, 0, 56],[117, 269, 360], + [118, 0, 57],[118, 269, 360], + [119, 0, 58],[119, 269, 360], + [120, 0, 59],[120, 269, 360], + [121, 0, 61],[121, 269, 360], + [122, 0, 63],[122, 268, 360], + [123, 0, 66],[123, 268, 360], + [124, 0, 69],[124, 268, 360], + [125, 0, 69],[125, 267, 360], + [126, 0, 71],[126, 267, 360], + [127, 0, 72],[127, 267, 360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml new file mode 100644 index 00000000..b1de13ab --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml @@ -0,0 +1,159 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 15], + [1, 0, 15], + [2, 0, 15], + [3, 0, 15], + [23, 0, 10], + [24, 0, 10], + [25, 0, 10], + [26, 0, 10], + [27, 0, 10], + [28, 0, 10], + [29, 0, 10], + [30, 0, 15], + [31, 0, 15], + [32, 0, 15], + [33, 0, 10], + [34, 0, 10], + [35, 0, 10], + [36, 0, 10], + [37, 0, 10], + [38, 0, 10], + [39, 0, 10], + [40, 0, 10], + [41, 0, 10], + [42, 0, 10], + [43, 0, 10], + [44, 0, 10], + [45, 0, 25], + [46, 0, 25], + [47, 0, 25], + [48, 0, 26], + [49, 0, 26], + [50, 0, 27], + [51, 0, 40], + [52, 0, 40], + [53, 0, 40], + [54, 0, 40], + [55, 0, 40], + [56, 0, 40], + [57, 0, 45], + [58, 0, 45], + [59, 0, 45], + [60, 0, 45], + [61, 0, 40], + [62, 0, 35], + [63, 0, 35], + [64, 0, 35], + [65, 0, 35], + [66, 0, 40], + [67, 0, 40], + [68, 0, 42], + [69, 0, 42], + [70, 0, 42], + [71, 0, 44], + [72, 0, 44], + [73, 0, 44], + [74, 0, 44], + [75, 0, 44], + [76, 0, 45], + [77, 0, 45], + [78, 0, 45], + [79, 0, 45], + [80, 0, 45], + [81, 0, 45], + [82, 0, 45], + [83, 0, 45], + [84, 0, 45], + [85, 0, 45], + [86, 0, 45], + [87, 0, 45], + [88, 0, 45], + [89, 0, 45], + [90, 0, 45], + [91, 0, 45], + [92, 0, 45], + [93, 0, 44], + [94, 0, 44], + [95, 0, 44], + [96, 0, 44], + [97, 0, 44], + [98, 0, 44], + [99, 0, 44], + [100, 0, 46], + [101, 0, 46], + [102, 0, 46], + [103, 0, 44], + [104, 0, 44],[104, 269, 360], + [105, 0, 44],[105, 269, 360], + [106, 0, 44],[106, 269, 360], + [107, 0, 44],[107, 269, 360], + [108, 0, 46],[108, 269, 360], + [109, 0, 47],[109, 269, 360], + [110, 0, 48],[110, 87.78, 89.97],[110, 269, 360], + [111, 0, 49],[111, 269, 360], + [112, 0, 50],[112, 269, 360], + [113, 0, 52],[113, 269, 360], + [114, 0, 53],[114, 269, 360], + [115, 0, 54],[115, 269, 360], + [116, 0, 55],[116, 269, 360], + [117, 0, 56],[117, 269, 360], + [118, 0, 57],[118, 269, 360], + [119, 0, 58],[119, 269, 360], + [120, 0, 59],[120, 269, 360], + [121, 0, 61],[121, 269, 360], + [122, 0, 63],[122, 268, 360], + [123, 0, 66],[123, 268, 360], + [124, 0, 69],[124, 268, 360], + [125, 0, 69],[125, 267, 360], + [126, 0, 71],[126, 267, 360], + [127, 0, 72],[127, 267, 360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index 6315c1b3..ee892983 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -30,7 +30,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index 8b0276d5..64825178 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -31,7 +31,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 5224e317..512c84f7 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -55,7 +55,7 @@ - + @@ -70,7 +70,7 @@ - + @@ -89,7 +89,7 @@ - + @@ -123,6 +123,7 @@ + @@ -156,6 +157,7 @@ + @@ -170,8 +172,8 @@ - - + + @@ -190,7 +192,7 @@ - + @@ -224,7 +226,7 @@ - + @@ -258,6 +260,7 @@ + @@ -291,6 +294,7 @@ + From b9668ddac42591dd3d44c570095ce2bcc4db5a12 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Wed, 6 Nov 2024 10:53:54 +0900 Subject: [PATCH 22/38] feat(aip_x2_gen2_launch): enbale topic_state_monitor (#338) * enbale topic_state_monitor * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x2_gen2_launch/launch/sensing.launch.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/aip_x2_gen2_launch/launch/sensing.launch.xml b/aip_x2_gen2_launch/launch/sensing.launch.xml index 68f655b5..ec9adc0e 100644 --- a/aip_x2_gen2_launch/launch/sensing.launch.xml +++ b/aip_x2_gen2_launch/launch/sensing.launch.xml @@ -2,7 +2,7 @@ - + @@ -35,6 +35,10 @@ + + + + From 8e112d47a90fe34043c02086bc99c99de3174d74 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Wed, 6 Nov 2024 17:07:17 +0900 Subject: [PATCH 23/38] feat(aip_x2_gen2_launch): add sync_angle and cut angle (#339) --- aip_x2_gen2_launch/launch/hesai_OT128.launch.xml | 6 ++++-- aip_x2_gen2_launch/launch/hesai_QT128.launch.xml | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index ee892983..e9cd6805 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -9,7 +9,8 @@ - + + @@ -40,7 +41,8 @@ - + + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index 64825178..80c73b0b 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -9,7 +9,8 @@ - + + @@ -41,7 +42,8 @@ - + + From 20d492db6e328f24cb8188b7ce455c73c5390736 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Fri, 8 Nov 2024 11:51:06 +0900 Subject: [PATCH 24/38] fix(aip_x2_gen2_launch): fix lidar diag name for dummy_diag_publisher (#340) fix lidar diag name --- .../sensor_kit.param.yaml | 48 +++++++++---------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index dd09b44e..7686ebf3 100644 --- a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -22,32 +22,32 @@ "gyro_bias_validator: gyro_bias_validator": default # lidar - "pandar_monitor: /sensing/lidar/front_lower: pandar_connection": default - "pandar_monitor: /sensing/lidar/front_upper: pandar_connection": default - "pandar_monitor: /sensing/lidar/left_lower: pandar_connection": default - "pandar_monitor: /sensing/lidar/left_upper: pandar_connection": default - "pandar_monitor: /sensing/lidar/right_lower: pandar_connection": default - "pandar_monitor: /sensing/lidar/right_upper: pandar_connection": default - "pandar_monitor: /sensing/lidar/rear_lower: pandar_connection": default - "pandar_monitor: /sensing/lidar/rear_upper: pandar_connection": default + "/sensing/lidar/front_lower/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/front_upper/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/left_lower/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/left_upper/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/right_lower/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/right_upper/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/rear_lower/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/rear_upper/hesai_ros_wrapper_node: hesai_status": default - "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp": default - "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp": default - "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp": default - "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": default + "/sensing/lidar/front_lower/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/front_upper/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/left_lower/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/left_upper/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/right_lower/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/right_upper/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/rear_lower/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/rear_upper/hesai_ros_wrapper_node: hesai_ptp": default - "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature": default - "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature": default - "pandar_monitor: /sensing/lidar/left_lower: pandar_temperature": default - "pandar_monitor: /sensing/lidar/left_upper: pandar_temperature": default - "pandar_monitor: /sensing/lidar/right_lower: pandar_temperature": default - "pandar_monitor: /sensing/lidar/right_upper: pandar_temperature": default - "pandar_monitor: /sensing/lidar/rear_lower: pandar_temperature": default - "pandar_monitor: /sensing/lidar/rear_upper: pandar_temperature": default + "/sensing/lidar/front_lower/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/front_upper/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/left_lower/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/left_upper/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/right_lower/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/right_upper/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/rear_lower/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/rear_upper/hesai_ros_wrapper_node: hesai_temperature": default "concatenate_data: concat_status": default # camera From c053877ce66e072fa9c9d0c2ac04e58ba19970b7 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Wed, 20 Nov 2024 11:50:16 +0900 Subject: [PATCH 25/38] feat(aip_x2_gen2_launch): remove crop box filter (#341) * remove crop box filter * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x2_gen2_launch/launch/nebula_node_container.launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index f76911eb..264f4182 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -206,7 +206,7 @@ def str2vector(string): remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("~/input/imu", "/sensing/imu/imu_data"), - ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/input/pointcloud", "pointcloud_raw_ex"), ("~/output/pointcloud", "rectified/pointcloud_ex"), ], parameters=[load_composable_node_param("distortion_corrector_node_param_file")], @@ -274,9 +274,9 @@ def str2vector(string): composable_node_descriptions=[ glog_component, nebula_component, - self_crop_component, - right_mirror_crop_component, - left_mirror_crop_component, + # self_crop_component, + # right_mirror_crop_component, + # left_mirror_crop_component, undistort_component, ], ) From 92f7e9c3dc89344368106b484325dac5a271581c Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Thu, 21 Nov 2024 13:29:18 +0900 Subject: [PATCH 26/38] feat(aip_x2_gen2_launch): enable self_crop_component (#343) enable self_crop_component --- aip_x2_gen2_launch/launch/nebula_node_container.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index 264f4182..d228e11c 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -206,7 +206,7 @@ def str2vector(string): remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("~/input/imu", "/sensing/imu/imu_data"), - ("~/input/pointcloud", "pointcloud_raw_ex"), + ("~/input/pointcloud", "self_cropped/pointcloud_ex"), ("~/output/pointcloud", "rectified/pointcloud_ex"), ], parameters=[load_composable_node_param("distortion_corrector_node_param_file")], @@ -274,7 +274,7 @@ def str2vector(string): composable_node_descriptions=[ glog_component, nebula_component, - # self_crop_component, + self_crop_component, # right_mirror_crop_component, # left_mirror_crop_component, undistort_component, From 0a143d3e4dafc652b63f92b32c1fb74b97304645 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Fri, 22 Nov 2024 11:21:28 +0900 Subject: [PATCH 27/38] fix(aip_x2_gen2_launch): update point_filters with MJ20-003 rosbag (#344) * fix front_upper lidar FOV * fix right_upper lidar FOV * fix right_lower lidar FOV * fix left_lower lidar FOV * fix right_upper lidar FOV * fix front_lower lidar FOV * fix front_upper lidar FOV * fix rear_upper lidar FOV * fix rear_upper point_filters config * fix point_filters config(correct filter startend angle) --- .../front_lower/point_filters_full.param.yaml | 71 +++--- .../point_filters_one_third.param.yaml | 71 +++--- .../point_filters_two_third.param.yaml | 71 +++--- .../front_upper/point_filters_full.param.yaml | 161 ++++++++----- .../point_filters_one_third.param.yaml | 161 ++++++++----- .../point_filters_two_third.param.yaml | 161 ++++++++----- .../left_lower/point_filters_full.param.yaml | 58 ++--- .../point_filters_one_third.param.yaml | 60 ++--- .../point_filters_two_third.param.yaml | 60 ++--- .../rear_lower/point_filters_full.param.yaml | 96 ++++---- .../point_filters_one_third.param.yaml | 96 ++++---- .../point_filters_two_third.param.yaml | 96 ++++---- .../rear_upper/point_filters_full.param.yaml | 224 +++++++++--------- .../point_filters_one_third.param.yaml | 224 +++++++++--------- .../point_filters_two_third.param.yaml | 224 +++++++++--------- .../right_lower/point_filters_full.param.yaml | 100 ++++---- .../point_filters_one_third.param.yaml | 100 ++++---- .../point_filters_two_third.param.yaml | 100 ++++---- .../right_upper/point_filters_full.param.yaml | 80 ++++--- .../point_filters_one_third.param.yaml | 80 ++++--- .../point_filters_two_third.param.yaml | 80 ++++--- 21 files changed, 1265 insertions(+), 1109 deletions(-) diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml index 33d53465..cd534e3a 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml @@ -3,39 +3,42 @@ point_filters: | { "ring_section_filter": [ - [0, 60,128], [0, 232,300], - [1, 60,124], [1, 236,300], - [2, 60,122], [2, 238,300], - [3, 60,120], [3, 240,300], - [4, 60,118], [4, 242,300], - [5, 60,118], [5, 242,300], - [6, 60,114], [6, 246,300], - [7, 60,114], [7, 246,300], - [8, 60,112], [8, 248,300], - [9, 60,112], [9, 248,300], - [10, 60,110], [10, 250,300], - [11, 60,110], [11, 250,300], - [12, 60,108], [12, 252,300], - [13, 60,108], [13, 252,300], - [14, 60,108], [14, 252,300], - [15, 60,108], [15, 252,300], - [16, 60,108], [16, 252,300], - [17, 60,108], [17, 252,300], - [18, 60,106], [18, 254,300], - [19, 60,106], [19, 254,300], - [20, 60,102], [20, 258,300], - [21, 60,102], [21, 258,300], - [22, 60,100], [22, 260,300], - [23, 60,100], [23, 260,300], - [24, 60,100], [24, 260,300], - [25, 60,100], [25, 260,300], - [26, 60,100], [26, 260,300], - [27, 60,100], [27, 260,300], - [28, 60,100], [28, 260,300], - [29, 60,100], [29, 260,300], - [30, 60,94], [30, 266,300], - [31, 60,94], [31, 266,300], - [32, 60,94], [32, 266,300], - [33, 60,94], [33, 266,300] + [0, 0, 128], [0, 232,360], + [1, 0, 128], [1, 236,360], + [2, 0, 125], [2, 238,360], + [3, 0, 123], [3, 240,360], + [4, 0, 121], [4, 242,360], + [5, 0, 121], [5, 242,360], + [6, 0, 120], [6, 246,360], + [7, 0, 120], [7, 246,360], + [8, 0, 120], [8, 248,360], + [9, 0, 115], [9, 248,360], + [10, 0, 115], [10, 250,360], + [11, 0, 115], [11, 250,360], + [12, 0, 115], [12, 252,360], + [13, 0, 110], [13, 252,360], + [14, 0, 108], [14, 252,360], + [15, 0, 108], [15, 252,360], + [16, 0, 108], [16, 252,360], + [17, 0, 108], [17, 252,360], + [18, 0, 106], [18, 254,360], + [19, 0, 106], [19, 254,360], + [20, 0, 105], [20, 258,360], + [21, 0, 105], [21, 258,360], + [22, 0, 105], [22, 260,360], + [23, 0, 105], [23, 260,360], + [24, 0, 103], [24, 260,360], + [25, 0, 103], [25, 260,360], + [26, 0, 100], [26, 260,360], + [27, 0, 100], [27, 260,360], + [28, 0, 100], [28, 260,360], + [29, 0, 100], [29, 260,360], + [30, 0, 94], [30, 266,360], + [31, 0, 94], [31, 266,360], + [32, 0, 94], [32, 266,360], + [33, 0, 94], [33, 266,360], + [34, 0, 94], [34, 266,360], + [35, 0, 94], [35, 266,360], + [36, 0, 94], [36, 266,360] ] } diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml index 71ba0993..f0607ac6 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml @@ -3,40 +3,43 @@ point_filters: | { "ring_section_filter": [ - [0, 60,128], [0, 232,300], - [1, 60,124], [1, 236,300], - [2, 60,122], [2, 238,300], - [3, 60,120], [3, 240,300], - [4, 60,118], [4, 242,300], - [5, 60,118], [5, 242,300], - [6, 60,114], [6, 246,300], - [7, 60,114], [7, 246,300], - [8, 60,112], [8, 248,300], - [9, 60,112], [9, 248,300], - [10, 60,110], [10, 250,300], - [11, 60,110], [11, 250,300], - [12, 60,108], [12, 252,300], - [13, 60,108], [13, 252,300], - [14, 60,108], [14, 252,300], - [15, 60,108], [15, 252,300], - [16, 60,108], [16, 252,300], - [17, 60,108], [17, 252,300], - [18, 60,106], [18, 254,300], - [19, 60,106], [19, 254,300], - [20, 60,102], [20, 258,300], - [21, 60,102], [21, 258,300], - [22, 60,100], [22, 260,300], - [23, 60,100], [23, 260,300], - [24, 60,100], [24, 260,300], - [25, 60,100], [25, 260,300], - [26, 60,100], [26, 260,300], - [27, 60,100], [27, 260,300], - [28, 60,100], [28, 260,300], - [29, 60,100], [29, 260,300], - [30, 60,94], [30, 266,300], - [31, 60,94], [31, 266,300], - [32, 60,94], [32, 266,300], - [33, 60,94], [33, 266,300], + [0, 0, 128], [0, 232,360], + [1, 0, 128], [1, 236,360], + [2, 0, 125], [2, 238,360], + [3, 0, 123], [3, 240,360], + [4, 0, 121], [4, 242,360], + [5, 0, 121], [5, 242,360], + [6, 0, 120], [6, 246,360], + [7, 0, 120], [7, 246,360], + [8, 0, 120], [8, 248,360], + [9, 0, 115], [9, 248,360], + [10, 0, 115], [10, 250,360], + [11, 0, 115], [11, 250,360], + [12, 0, 115], [12, 252,360], + [13, 0, 110], [13, 252,360], + [14, 0, 108], [14, 252,360], + [15, 0, 108], [15, 252,360], + [16, 0, 108], [16, 252,360], + [17, 0, 108], [17, 252,360], + [18, 0, 106], [18, 254,360], + [19, 0, 106], [19, 254,360], + [20, 0, 105], [20, 258,360], + [21, 0, 105], [21, 258,360], + [22, 0, 105], [22, 260,360], + [23, 0, 105], [23, 260,360], + [24, 0, 103], [24, 260,360], + [25, 0, 103], [25, 260,360], + [26, 0, 100], [26, 260,360], + [27, 0, 100], [27, 260,360], + [28, 0, 100], [28, 260,360], + [29, 0, 100], [29, 260,360], + [30, 0, 94], [30, 266,360], + [31, 0, 94], [31, 266,360], + [32, 0, 94], [32, 266,360], + [33, 0, 94], [33, 266,360], + [34, 0, 94], [34, 266,360], + [35, 0, 94], [35, 266,360], + [36, 0, 94], [36, 266,360], [1, 0, 360], [2, 0, 360], [4, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml index 0ccc1830..dc4e10a9 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml @@ -3,40 +3,43 @@ point_filters: | { "ring_section_filter": [ - [0, 60,128], [0, 232,300], - [1, 60,124], [1, 236,300], - [2, 60,122], [2, 238,300], - [3, 60,120], [3, 240,300], - [4, 60,118], [4, 242,300], - [5, 60,118], [5, 242,300], - [6, 60,114], [6, 246,300], - [7, 60,114], [7, 246,300], - [8, 60,112], [8, 248,300], - [9, 60,112], [9, 248,300], - [10, 60,110], [10, 250,300], - [11, 60,110], [11, 250,300], - [12, 60,108], [12, 252,300], - [13, 60,108], [13, 252,300], - [14, 60,108], [14, 252,300], - [15, 60,108], [15, 252,300], - [16, 60,108], [16, 252,300], - [17, 60,108], [17, 252,300], - [18, 60,106], [18, 254,300], - [19, 60,106], [19, 254,300], - [20, 60,102], [20, 258,300], - [21, 60,102], [21, 258,300], - [22, 60,100], [22, 260,300], - [23, 60,100], [23, 260,300], - [24, 60,100], [24, 260,300], - [25, 60,100], [25, 260,300], - [26, 60,100], [26, 260,300], - [27, 60,100], [27, 260,300], - [28, 60,100], [28, 260,300], - [29, 60,100], [29, 260,300], - [30, 60,94], [30, 266,300], - [31, 60,94], [31, 266,300], - [32, 60,94], [32, 266,300], - [33, 60,94], [33, 266,300], + [0, 0, 128], [0, 232,360], + [1, 0, 128], [1, 236,360], + [2, 0, 125], [2, 238,360], + [3, 0, 123], [3, 240,360], + [4, 0, 121], [4, 242,360], + [5, 0, 121], [5, 242,360], + [6, 0, 120], [6, 246,360], + [7, 0, 120], [7, 246,360], + [8, 0, 120], [8, 248,360], + [9, 0, 115], [9, 248,360], + [10, 0, 115], [10, 250,360], + [11, 0, 115], [11, 250,360], + [12, 0, 115], [12, 252,360], + [13, 0, 110], [13, 252,360], + [14, 0, 108], [14, 252,360], + [15, 0, 108], [15, 252,360], + [16, 0, 108], [16, 252,360], + [17, 0, 108], [17, 252,360], + [18, 0, 106], [18, 254,360], + [19, 0, 106], [19, 254,360], + [20, 0, 105], [20, 258,360], + [21, 0, 105], [21, 258,360], + [22, 0, 105], [22, 260,360], + [23, 0, 105], [23, 260,360], + [24, 0, 103], [24, 260,360], + [25, 0, 103], [25, 260,360], + [26, 0, 100], [26, 260,360], + [27, 0, 100], [27, 260,360], + [28, 0, 100], [28, 260,360], + [29, 0, 100], [29, 260,360], + [30, 0, 94], [30, 266,360], + [31, 0, 94], [31, 266,360], + [32, 0, 94], [32, 266,360], + [33, 0, 94], [33, 266,360], + [34, 0, 94], [34, 266,360], + [35, 0, 94], [35, 266,360], + [36, 0, 94], [36, 266,360], [0, 0, 360], [3, 0, 360], [6, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml index 86c11c7f..630b2ae4 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml @@ -3,70 +3,101 @@ point_filters: | { "ring_section_filter": [ - [0, 60, 93],[0, 266, 300], - [1, 60, 93],[1, 266, 300], - [2, 60, 93],[2, 266, 300], - [3, 60, 93],[3, 266, 300], - [4, 268, 300], - [5, 268, 300], - [6, 268, 300], - [7, 268, 300], - [8, 268, 300], - [10, 272, 300], - [11, 272, 300], - [12, 272, 300], - [13, 272, 300], - [14, 266, 300], - [15, 266, 300], - [16, 266, 300], - [18, 263, 300], - [19, 263, 300], - [20, 263, 300], - [22, 270, 300], - [23, 270, 300], - [24, 270, 300], - [30, 267, 300], - [31, 267, 300], - [32, 267, 300], - [38, 272, 300], - [45, 60, 93],[45, 267, 300], - [52, 195, 300], - [58, 261, 300], - [59, 261, 300], - [60, 261, 300], - [72, 269, 300], - [73, 269, 300], - [74, 269, 300], - [88, 273, 300], - [89, 273, 300], - [90, 273, 300], - [91, 60, 93],[91, 273, 300], - [92, 60, 93],[92, 268, 300], - [93, 60, 93],[93, 273, 300], - [94, 60, 93],[94, 273, 300], - [95, 60, 93],[95, 273, 300], - [96, 60, 93],[96, 273, 300], - [97, 60, 93],[97, 273, 300], - [98, 60, 93],[98, 273, 300], - [99, 60, 93],[99, 273, 300], - [100, 60, 93.5],[100, 273, 300], - [101, 273, 300], - [102, 273, 300], - [103, 274, 300], - [104, 274, 300], - [105, 275, 300], - [106, 275, 300], - [107, 275, 300], - [108, 275, 300], - [112, 60, 93], - [116, 60, 93], - [120, 60, 93],[120, 265, 300], - [121, 60, 93],[121, 265, 300], - [122, 60, 93],[122, 265, 300], - [123, 60, 94],[123, 265, 300], - [124, 60, 94],[124, 265, 300], - [125, 60, 97],[125, 256, 300], - [126, 60, 97],[126, 256, 300], - [127, 60, 97],[127, 256, 300] + [0, 0, 93],[0, 266, 360], + [1, 0, 93],[1, 266, 360], + [2, 0, 93],[2, 266, 360], + [3, 0, 93],[3, 266, 360], + [4, 268, 360], + [5, 268, 360], + [6, 268, 360], + [7, 268, 360], + [8, 268, 360], + [10, 272, 360], + [11, 272, 360], + [12, 272, 360], + [13, 272, 360], + [14, 266, 360], + [15, 266, 360], + [16, 266, 360], + [17, 0, 96],[17, 266, 360], + [18, 0, 96],[18, 263, 360], + [19, 0, 96],[19, 263, 360], + [20, 0, 96],[20, 263, 360], + [21, 0, 96],[21, 263, 360], + [22, 270, 360], + [23, 270, 360], + [24, 270, 360], + [29, 0, 96], + [30, 0, 96],[30, 267, 360], + [31, 0, 96],[31, 267, 360], + [32, 0, 96],[32, 267, 360], + [33, 0, 96], + [38, 272, 360], + [45, 0, 93],[45, 267, 360], + [50, 0, 98], + [51, 0, 98], + [52, 0, 98],[52, 195, 360], + [53, 0, 98], + [54, 0, 98], + [57, 0, 93], + [58, 0, 93],[58, 261, 360], + [59, 0, 93],[59, 261, 360], + [60, 0, 93],[60, 261, 360], + [61, 0, 93], + [72, 269, 360], + [73, 0, 88],[73, 269, 360], + [74, 0, 88],[74, 269, 360], + [75, 0, 90], + [76, 0, 94], + [77, 0, 94], + [78, 0, 94], + [79, 0, 94], + [80, 0, 94], + [81, 0, 90], + [82, 0, 90], + [83, 0, 90], + [84, 0, 88], + [85, 0, 88], + [86, 0, 88], + [88, 273, 360], + [89, 273, 360], + [90, 273, 360], + [91, 0, 93],[91, 273, 360], + [92, 0, 93],[92, 268, 360], + [93, 0, 93],[93, 273, 360], + [94, 0, 93],[94, 273, 360], + [95, 0, 93],[95, 273, 360], + [96, 0, 93],[96, 273, 360], + [97, 0, 93],[97, 273, 360], + [98, 0, 93],[98, 273, 360], + [99, 0, 93],[99, 273, 360], + [100, 0, 93.5],[100, 273, 360], + [101, 273, 360], + [102, 273, 360], + [103, 274, 360], + [104, 274, 360], + [105, 275, 360], + [106, 0, 95],[106, 275, 360], + [107, 0, 95],[107, 275, 360], + [108, 0, 95],[108, 275, 360], + [109, 0, 95], + [110, 0, 95], + [111, 0, 90], + [112, 0, 93], + [113, 0, 93], + [114, 0, 93], + [115, 0, 93], + [116, 0, 93], + [117, 0, 93], + [118, 0, 96],[118, 265, 360], + [119, 0, 96],[119, 265, 360], + [120, 0, 98],[120, 265, 360], + [121, 0, 98],[121, 265, 360], + [122, 0, 98],[122, 258, 360], + [123, 0, 98],[123, 258, 360], + [124, 0, 98],[124, 258, 360], + [125, 0, 100],[125, 256, 360], + [126, 0, 100],[126, 256, 360], + [127, 0, 100],[127, 256, 360] ] } diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml index a61e40e8..ddb36703 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml @@ -3,71 +3,102 @@ point_filters: | { "ring_section_filter": [ - [0, 60, 93],[0, 266, 300], - [1, 60, 93],[1, 266, 300], - [2, 60, 93],[2, 266, 300], - [3, 60, 93],[3, 266, 300], - [4, 268, 300], - [5, 268, 300], - [6, 268, 300], - [7, 268, 300], - [8, 268, 300], - [10, 272, 300], - [11, 272, 300], - [12, 272, 300], - [13, 272, 300], - [14, 266, 300], - [15, 266, 300], - [16, 266, 300], - [18, 263, 300], - [19, 263, 300], - [20, 263, 300], - [22, 270, 300], - [23, 270, 300], - [24, 270, 300], - [30, 267, 300], - [31, 267, 300], - [32, 267, 300], - [38, 272, 300], - [45, 60, 93],[45, 267, 300], - [52, 195, 300], - [58, 261, 300], - [59, 261, 300], - [60, 261, 300], - [72, 269, 300], - [73, 269, 300], - [74, 269, 300], - [88, 273, 300], - [89, 273, 300], - [90, 273, 300], - [91, 60, 93],[91, 273, 300], - [92, 60, 93],[92, 268, 300], - [93, 60, 93],[93, 273, 300], - [94, 60, 93],[94, 273, 300], - [95, 60, 93],[95, 273, 300], - [96, 60, 93],[96, 273, 300], - [97, 60, 93],[97, 273, 300], - [98, 60, 93],[98, 273, 300], - [99, 60, 93],[99, 273, 300], - [100, 60, 93.5],[100, 273, 300], - [101, 273, 300], - [102, 273, 300], - [103, 274, 300], - [104, 274, 300], - [105, 275, 300], - [106, 275, 300], - [107, 275, 300], - [108, 275, 300], - [112, 60, 93], - [116, 60, 93], - [120, 60, 93],[120, 265, 300], - [121, 60, 93],[121, 265, 300], - [122, 60, 93],[122, 265, 300], - [123, 60, 94],[123, 265, 300], - [124, 60, 94],[124, 265, 300], - [125, 60, 97],[125, 256, 300], - [126, 60, 97],[126, 256, 300], - [127, 60, 97],[127, 256, 300], + [0, 0, 93],[0, 266, 360], + [1, 0, 93],[1, 266, 360], + [2, 0, 93],[2, 266, 360], + [3, 0, 93],[3, 266, 360], + [4, 268, 360], + [5, 268, 360], + [6, 268, 360], + [7, 268, 360], + [8, 268, 360], + [10, 272, 360], + [11, 272, 360], + [12, 272, 360], + [13, 272, 360], + [14, 266, 360], + [15, 266, 360], + [16, 266, 360], + [17, 0, 96],[17, 266, 360], + [18, 0, 96],[18, 263, 360], + [19, 0, 96],[19, 263, 360], + [20, 0, 96],[20, 263, 360], + [21, 0, 96],[21, 263, 360], + [22, 270, 360], + [23, 270, 360], + [24, 270, 360], + [29, 0, 96], + [30, 0, 96],[30, 267, 360], + [31, 0, 96],[31, 267, 360], + [32, 0, 96],[32, 267, 360], + [33, 0, 96], + [38, 272, 360], + [45, 0, 93],[45, 267, 360], + [50, 0, 98], + [51, 0, 98], + [52, 0, 98],[52, 195, 360], + [53, 0, 98], + [54, 0, 98], + [57, 0, 93], + [58, 0, 93],[58, 261, 360], + [59, 0, 93],[59, 261, 360], + [60, 0, 93],[60, 261, 360], + [61, 0, 93], + [72, 269, 360], + [73, 0, 88],[73, 269, 360], + [74, 0, 88],[74, 269, 360], + [75, 0, 90], + [76, 0, 94], + [77, 0, 94], + [78, 0, 94], + [79, 0, 94], + [80, 0, 94], + [81, 0, 90], + [82, 0, 90], + [83, 0, 90], + [84, 0, 88], + [85, 0, 88], + [86, 0, 88], + [88, 273, 360], + [89, 273, 360], + [90, 273, 360], + [91, 0, 93],[91, 273, 360], + [92, 0, 93],[92, 268, 360], + [93, 0, 93],[93, 273, 360], + [94, 0, 93],[94, 273, 360], + [95, 0, 93],[95, 273, 360], + [96, 0, 93],[96, 273, 360], + [97, 0, 93],[97, 273, 360], + [98, 0, 93],[98, 273, 360], + [99, 0, 93],[99, 273, 360], + [100, 0, 93.5],[100, 273, 360], + [101, 273, 360], + [102, 273, 360], + [103, 274, 360], + [104, 274, 360], + [105, 275, 360], + [106, 0, 95],[106, 275, 360], + [107, 0, 95],[107, 275, 360], + [108, 0, 95],[108, 275, 360], + [109, 0, 95], + [110, 0, 95], + [111, 0, 90], + [112, 0, 93], + [113, 0, 93], + [114, 0, 93], + [115, 0, 93], + [116, 0, 93], + [117, 0, 93], + [118, 0, 96],[118, 265, 360], + [119, 0, 96],[119, 265, 360], + [120, 0, 98],[120, 265, 360], + [121, 0, 98],[121, 265, 360], + [122, 0, 98],[122, 258, 360], + [123, 0, 98],[123, 258, 360], + [124, 0, 98],[124, 258, 360], + [125, 0, 100],[125, 256, 360], + [126, 0, 100],[126, 256, 360], + [127, 0, 100],[127, 256, 360], [1, 0, 360], [2, 0, 360], [4, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml index ee82ff19..02a1e4ee 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml @@ -3,71 +3,102 @@ point_filters: | { "ring_section_filter": [ - [0, 60, 93],[0, 266, 300], - [1, 60, 93],[1, 266, 300], - [2, 60, 93],[2, 266, 300], - [3, 60, 93],[3, 266, 300], - [4, 268, 300], - [5, 268, 300], - [6, 268, 300], - [7, 268, 300], - [8, 268, 300], - [10, 272, 300], - [11, 272, 300], - [12, 272, 300], - [13, 272, 300], - [14, 266, 300], - [15, 266, 300], - [16, 266, 300], - [18, 263, 300], - [19, 263, 300], - [20, 263, 300], - [22, 270, 300], - [23, 270, 300], - [24, 270, 300], - [30, 267, 300], - [31, 267, 300], - [32, 267, 300], - [38, 272, 300], - [45, 60, 93],[45, 267, 300], - [52, 195, 300], - [58, 261, 300], - [59, 261, 300], - [60, 261, 300], - [72, 269, 300], - [73, 269, 300], - [74, 269, 300], - [88, 273, 300], - [89, 273, 300], - [90, 273, 300], - [91, 60, 93],[91, 273, 300], - [92, 60, 93],[92, 268, 300], - [93, 60, 93],[93, 273, 300], - [94, 60, 93],[94, 273, 300], - [95, 60, 93],[95, 273, 300], - [96, 60, 93],[96, 273, 300], - [97, 60, 93],[97, 273, 300], - [98, 60, 93],[98, 273, 300], - [99, 60, 93],[99, 273, 300], - [100, 60, 93.5],[100, 273, 300], - [101, 273, 300], - [102, 273, 300], - [103, 274, 300], - [104, 274, 300], - [105, 275, 300], - [106, 275, 300], - [107, 275, 300], - [108, 275, 300], - [112, 60, 93], - [116, 60, 93], - [120, 60, 93],[120, 265, 300], - [121, 60, 93],[121, 265, 300], - [122, 60, 93],[122, 265, 300], - [123, 60, 94],[123, 265, 300], - [124, 60, 94],[124, 265, 300], - [125, 60, 97],[125, 256, 300], - [126, 60, 97],[126, 256, 300], - [127, 60, 97],[127, 256, 300], + [0, 0, 93],[0, 266, 360], + [1, 0, 93],[1, 266, 360], + [2, 0, 93],[2, 266, 360], + [3, 0, 93],[3, 266, 360], + [4, 268, 360], + [5, 268, 360], + [6, 268, 360], + [7, 268, 360], + [8, 268, 360], + [10, 272, 360], + [11, 272, 360], + [12, 272, 360], + [13, 272, 360], + [14, 266, 360], + [15, 266, 360], + [16, 266, 360], + [17, 0, 96],[17, 266, 360], + [18, 0, 96],[18, 263, 360], + [19, 0, 96],[19, 263, 360], + [20, 0, 96],[20, 263, 360], + [21, 0, 96],[21, 263, 360], + [22, 270, 360], + [23, 270, 360], + [24, 270, 360], + [29, 0, 96], + [30, 0, 96],[30, 267, 360], + [31, 0, 96],[31, 267, 360], + [32, 0, 96],[32, 267, 360], + [33, 0, 96], + [38, 272, 360], + [45, 0, 93],[45, 267, 360], + [50, 0, 98], + [51, 0, 98], + [52, 0, 98],[52, 195, 360], + [53, 0, 98], + [54, 0, 98], + [57, 0, 93], + [58, 0, 93],[58, 261, 360], + [59, 0, 93],[59, 261, 360], + [60, 0, 93],[60, 261, 360], + [61, 0, 93], + [72, 269, 360], + [73, 0, 88],[73, 269, 360], + [74, 0, 88],[74, 269, 360], + [75, 0, 90], + [76, 0, 94], + [77, 0, 94], + [78, 0, 94], + [79, 0, 94], + [80, 0, 94], + [81, 0, 90], + [82, 0, 90], + [83, 0, 90], + [84, 0, 88], + [85, 0, 88], + [86, 0, 88], + [88, 273, 360], + [89, 273, 360], + [90, 273, 360], + [91, 0, 93],[91, 273, 360], + [92, 0, 93],[92, 268, 360], + [93, 0, 93],[93, 273, 360], + [94, 0, 93],[94, 273, 360], + [95, 0, 93],[95, 273, 360], + [96, 0, 93],[96, 273, 360], + [97, 0, 93],[97, 273, 360], + [98, 0, 93],[98, 273, 360], + [99, 0, 93],[99, 273, 360], + [100, 0, 93.5],[100, 273, 360], + [101, 273, 360], + [102, 273, 360], + [103, 274, 360], + [104, 274, 360], + [105, 275, 360], + [106, 0, 95],[106, 275, 360], + [107, 0, 95],[107, 275, 360], + [108, 0, 95],[108, 275, 360], + [109, 0, 95], + [110, 0, 95], + [111, 0, 90], + [112, 0, 93], + [113, 0, 93], + [114, 0, 93], + [115, 0, 93], + [116, 0, 93], + [117, 0, 93], + [118, 0, 96],[118, 265, 360], + [119, 0, 96],[119, 265, 360], + [120, 0, 98],[120, 265, 360], + [121, 0, 98],[121, 265, 360], + [122, 0, 98],[122, 258, 360], + [123, 0, 98],[123, 258, 360], + [124, 0, 98],[124, 258, 360], + [125, 0, 100],[125, 256, 360], + [126, 0, 100],[126, 256, 360], + [127, 0, 100],[127, 256, 360], [0, 0, 360], [3, 0, 360], [6, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml index 2a2b0a5b..b0d6c29d 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml @@ -6,35 +6,35 @@ [0, 0, 360], [1, 0, 360], [2, 0, 360], - [3, 0, 170],[3, 199.5, 300], - [4, 0, 155],[4, 203, 300], - [5, 0, 152],[5, 206, 217],[5, 220, 300], - [6, 0, 146],[6, 217, 300], - [7, 0, 143],[7, 235, 300], - [8, 0, 143],[8, 228.5, 300], - [9, 0, 142],[9, 228.5, 300], - [10, 0, 141],[10, 235, 300], - [11, 0, 140],[11, 235, 300], - [12, 0, 140],[12, 235, 300], - [13, 0, 135],[13, 235, 300], - [14, 0, 133],[14, 235, 300], - [15, 0, 131],[15, 235, 300], - [16, 0, 129],[16, 235, 300], - [17, 0, 127],[17, 235, 300], - [18, 0, 127],[18, 235, 300], - [19, 0, 126],[19, 235, 300], - [20, 0, 125],[20, 235, 300], - [21, 0, 125],[21, 235, 300], - [22, 0, 124],[22, 235, 300], - [23, 0, 124],[23, 235, 300], - [24, 0, 122],[24, 235, 300], - [25, 0, 122],[25, 235, 300], - [26, 0, 121],[26, 235, 300], - [27, 0, 120],[27, 235, 300], - [28, 0, 119],[28, 242, 300], - [29, 0, 118],[29, 242, 300], - [30, 0, 110],[30, 242, 300], - [31, 0, 109],[31, 244, 265], + [3, 0, 170],[3, 199.5, 360], + [4, 0, 155],[4, 203, 360], + [5, 0, 152],[5, 206, 217],[5, 220, 360], + [6, 0, 146],[6, 217, 360], + [7, 0, 143],[7, 235, 360], + [8, 0, 143],[8, 228.5, 360], + [9, 0, 142],[9, 228.5, 360], + [10, 0, 141],[10, 235, 360], + [11, 0, 140],[11, 235, 360], + [12, 0, 140],[12, 235, 360], + [13, 0, 135],[13, 235, 360], + [14, 0, 133],[14, 235, 360], + [15, 0, 131],[15, 235, 360], + [16, 0, 129],[16, 235, 360], + [17, 0, 127],[17, 235, 360], + [18, 0, 127],[18, 235, 360], + [19, 0, 126],[19, 235, 360], + [20, 0, 125],[20, 235, 360], + [21, 0, 125],[21, 235, 360], + [22, 0, 124],[22, 235, 360], + [23, 0, 124],[23, 235, 360], + [24, 0, 122],[24, 235, 360], + [25, 0, 122],[25, 235, 360], + [26, 0, 121],[26, 235, 360], + [27, 0, 120],[27, 235, 360], + [28, 0, 119],[28, 240, 360], + [29, 0, 118],[29, 240, 360], + [30, 0, 110],[30, 240, 360], + [31, 0, 109],[31, 240, 265], [32, 0, 109],[32, 244, 265], [33, 0, 108],[33, 244, 265], [34, 0, 108],[34, 244, 265], diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml index d05dd26f..db47c2ac 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml @@ -5,36 +5,36 @@ "ring_section_filter": [ [0, 0, 360], [1, 0, 360], - [2, 0, 170],[2, 192, 300], - [3, 0, 160],[3, 199.5, 300], - [4, 0, 155],[4, 203, 300], - [5, 0, 152],[5, 206, 217],[5, 220, 300], - [6, 0, 146],[6, 217, 300], - [7, 0, 143],[7, 235, 300], - [8, 0, 139],[8, 228.5, 300], - [9, 0, 135],[9, 228.5, 300], - [10, 0, 133],[10, 235, 300], - [11, 0, 132],[11, 235, 300], - [12, 0, 131],[12, 235, 300], - [13, 0, 129],[13, 235, 300], - [14, 0, 127],[14, 235, 300], - [15, 0, 126],[15, 235, 300], - [16, 0, 124],[16, 235, 300], - [17, 0, 122],[17, 235, 300], - [18, 0, 121],[18, 235, 300], - [19, 0, 120],[19, 235, 300], - [20, 0, 119],[20, 235, 300], - [21, 0, 118],[21, 235, 300], - [22, 0, 117],[22, 235, 300], - [23, 0, 116],[23, 235, 300], - [24, 0, 115],[24, 235, 300], - [25, 0, 114],[25, 235, 300], - [26, 0, 113],[26, 235, 300], - [27, 0, 112],[27, 235, 300], - [28, 0, 111],[28, 242, 300], - [29, 0, 110],[29, 242, 300], - [30, 0, 110],[30, 242, 300], - [31, 0, 109],[31, 244, 265], + [2, 0, 170],[2, 192, 360], + [3, 0, 160],[3, 199.5, 360], + [4, 0, 155],[4, 203, 360], + [5, 0, 152],[5, 206, 217],[5, 220, 360], + [6, 0, 146],[6, 217, 360], + [7, 0, 143],[7, 235, 360], + [8, 0, 139],[8, 228.5, 360], + [9, 0, 135],[9, 228.5, 360], + [10, 0, 133],[10, 235, 360], + [11, 0, 132],[11, 235, 360], + [12, 0, 131],[12, 235, 360], + [13, 0, 129],[13, 235, 360], + [14, 0, 127],[14, 235, 360], + [15, 0, 126],[15, 235, 360], + [16, 0, 124],[16, 235, 360], + [17, 0, 122],[17, 235, 360], + [18, 0, 121],[18, 235, 360], + [19, 0, 120],[19, 235, 360], + [20, 0, 119],[20, 235, 360], + [21, 0, 118],[21, 235, 360], + [22, 0, 117],[22, 235, 360], + [23, 0, 116],[23, 235, 360], + [24, 0, 115],[24, 235, 360], + [25, 0, 114],[25, 235, 360], + [26, 0, 113],[26, 235, 360], + [27, 0, 112],[27, 235, 360], + [28, 0, 119],[28, 240, 360], + [29, 0, 118],[29, 240, 360], + [30, 0, 110],[30, 240, 360], + [31, 0, 109],[31, 240, 265], [32, 0, 109],[32, 244, 265], [33, 0, 108],[33, 244, 265], [34, 0, 108],[34, 244, 265], diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml index f7445d31..263deabf 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml @@ -5,36 +5,36 @@ "ring_section_filter": [ [0, 0, 360], [1, 0, 360], - [2, 0, 170],[2, 192, 300], - [3, 0, 160],[3, 199.5, 300], - [4, 0, 155],[4, 203, 300], - [5, 0, 152],[5, 206, 217],[5, 220, 300], - [6, 0, 146],[6, 217, 300], - [7, 0, 143],[7, 235, 300], - [8, 0, 139],[8, 228.5, 300], - [9, 0, 135],[9, 228.5, 300], - [10, 0, 133],[10, 235, 300], - [11, 0, 132],[11, 235, 300], - [12, 0, 131],[12, 235, 300], - [13, 0, 129],[13, 235, 300], - [14, 0, 127],[14, 235, 300], - [15, 0, 126],[15, 235, 300], - [16, 0, 124],[16, 235, 300], - [17, 0, 122],[17, 235, 300], - [18, 0, 121],[18, 235, 300], - [19, 0, 120],[19, 235, 300], - [20, 0, 119],[20, 235, 300], - [21, 0, 118],[21, 235, 300], - [22, 0, 117],[22, 235, 300], - [23, 0, 116],[23, 235, 300], - [24, 0, 115],[24, 235, 300], - [25, 0, 114],[25, 235, 300], - [26, 0, 113],[26, 235, 300], - [27, 0, 112],[27, 235, 300], - [28, 0, 111],[28, 242, 300], - [29, 0, 110],[29, 242, 300], - [30, 0, 110],[30, 242, 300], - [31, 0, 109],[31, 244, 265], + [2, 0, 170],[2, 192, 360], + [3, 0, 160],[3, 199.5, 360], + [4, 0, 155],[4, 203, 360], + [5, 0, 152],[5, 206, 217],[5, 220, 360], + [6, 0, 146],[6, 217, 360], + [7, 0, 143],[7, 235, 360], + [8, 0, 139],[8, 228.5, 360], + [9, 0, 135],[9, 228.5, 360], + [10, 0, 133],[10, 235, 360], + [11, 0, 132],[11, 235, 360], + [12, 0, 131],[12, 235, 360], + [13, 0, 129],[13, 235, 360], + [14, 0, 127],[14, 235, 360], + [15, 0, 126],[15, 235, 360], + [16, 0, 124],[16, 235, 360], + [17, 0, 122],[17, 235, 360], + [18, 0, 121],[18, 235, 360], + [19, 0, 120],[19, 235, 360], + [20, 0, 119],[20, 235, 360], + [21, 0, 118],[21, 235, 360], + [22, 0, 117],[22, 235, 360], + [23, 0, 116],[23, 235, 360], + [24, 0, 115],[24, 235, 360], + [25, 0, 114],[25, 235, 360], + [26, 0, 113],[26, 235, 360], + [27, 0, 112],[27, 235, 360], + [28, 0, 119],[28, 240, 360], + [29, 0, 118],[29, 240, 360], + [30, 0, 110],[30, 240, 360], + [31, 0, 109],[31, 240, 265], [32, 0, 109],[32, 244, 265], [33, 0, 108],[33, 244, 265], [34, 0, 108],[34, 244, 265], diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml index 770a6c07..ee183856 100644 --- a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml @@ -3,53 +3,53 @@ point_filters: | { "ring_section_filter": [ - [0, 60,148], [0, 212,300], - [1, 60,144], [1, 216,300], - [2, 60,142], [2, 218,300], - [3, 60,140], [3, 220,300], - [4, 60,138], [4, 222,300], - [5, 60,138], [5, 222,300], - [6, 60,134], [6, 226,300], - [7, 60,134], [7, 226,300], - [8, 60,132], [8, 228,300], - [9, 60,132], [9, 228,300], - [10, 60,130], [10, 230,300], - [11, 60,120], [11, 240,300], - [12, 60,118], [12, 242,300], - [13, 60,118], [13, 242,300], - [14, 60,118], [14, 242,300], - [15, 60,118], [15, 242,300], - [16, 60,118], [16, 242,300], - [17, 60,118], [17, 242,300], - [18, 60,116], [18, 244,300], - [19, 60,116], [19, 244,300], - [20, 60,112], [20, 248,300], - [21, 60,112], [21, 248,300], - [22, 60,110], [22, 250,300], - [23, 60,110], [23, 250,300], - [24, 60,110], [24, 250,300], - [25, 60,110], [25, 250,300], - [26, 60,110], [26, 250,300], - [27, 60,106], [27, 254,300], - [28, 60,106], [28, 254,300], - [29, 60,106], [29, 254,300], - [30, 60,104], [30, 256,300], - [31, 60,104], [31, 256,300], - [32, 60,104], [32, 256,300], - [33, 60,104], [33, 256,300], - [34, 60,104], [34, 256,300], - [35, 60,100], [35, 260,300], - [36, 60,100], [36, 260,300], - [37, 60,100], [37, 260,300], - [38, 60,100], [38, 260,300], - [39, 60,96], [39, 264,300], - [40, 60,96], [40, 264,300], - [41, 60,96], [41, 264,300], - [42, 60,96], [42, 264,300], - [43, 60,96], [43, 264,300], - [44, 60,94], [44, 266,300], - [45, 60,94], [45, 266,300], - [46, 60,94], [46, 266,300], - [47, 60,94], [47, 266,300] + [0, 60, 148], [0, 212,360], + [1, 60, 144], [1, 216,360], + [2, 60, 142], [2, 218,360], + [3, 60, 140], [3, 220,360], + [4, 60, 138], [4, 222,360], + [5, 60, 138], [5, 222,360], + [6, 60, 134], [6, 226,360], + [7, 60, 134], [7, 226,360], + [8, 60, 132], [8, 228,360], + [9, 60, 132], [9, 228,360], + [10, 60, 130], [10, 230,360], + [11, 60, 120], [11, 240,360], + [12, 60, 118], [12, 242,360], + [13, 60, 118], [13, 242,360], + [14, 60, 118], [14, 242,360], + [15, 60, 118], [15, 242,360], + [16, 60, 118], [16, 242,360], + [17, 60, 118], [17, 242,360], + [18, 60, 116], [18, 244,360], + [19, 60, 116], [19, 244,360], + [20, 60, 112], [20, 248,360], + [21, 60, 112], [21, 248,360], + [22, 60, 110], [22, 250,360], + [23, 60, 110], [23, 250,360], + [24, 60, 110], [24, 250,360], + [25, 60, 110], [25, 250,360], + [26, 60, 110], [26, 250,360], + [27, 60, 106], [27, 254,360], + [28, 60, 106], [28, 254,360], + [29, 60, 106], [29, 254,360], + [30, 60, 104], [30, 256,360], + [31, 60, 104], [31, 256,360], + [32, 60, 104], [32, 256,360], + [33, 60, 104], [33, 256,360], + [34, 60, 104], [34, 256,360], + [35, 60, 100], [35, 260,360], + [36, 60, 100], [36, 260,360], + [37, 60, 100], [37, 260,360], + [38, 60, 100], [38, 260,360], + [39, 60, 96], [39, 264,360], + [40, 60, 96], [40, 264,360], + [41, 60, 96], [41, 264,360], + [42, 60, 96], [42, 264,360], + [43, 60, 96], [43, 264,360], + [44, 60, 94], [44, 266,360], + [45, 60, 94], [45, 266,360], + [46, 60, 94], [46, 266,360], + [47, 60, 94], [47, 266,360] ] } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml index cc034395..d4fe2b58 100644 --- a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml @@ -3,54 +3,54 @@ point_filters: | { "ring_section_filter": [ - [0, 60,148], [0, 212,300], - [1, 60,144], [1, 216,300], - [2, 60,142], [2, 218,300], - [3, 60,140], [3, 220,300], - [4, 60,138], [4, 222,300], - [5, 60,138], [5, 222,300], - [6, 60,134], [6, 226,300], - [7, 60,134], [7, 226,300], - [8, 60,132], [8, 228,300], - [9, 60,132], [9, 228,300], - [10, 60,130], [10, 230,300], - [11, 60,120], [11, 240,300], - [12, 60,118], [12, 242,300], - [13, 60,118], [13, 242,300], - [14, 60,118], [14, 242,300], - [15, 60,118], [15, 242,300], - [16, 60,118], [16, 242,300], - [17, 60,118], [17, 242,300], - [18, 60,116], [18, 244,300], - [19, 60,116], [19, 244,300], - [20, 60,112], [20, 248,300], - [21, 60,112], [21, 248,300], - [22, 60,110], [22, 250,300], - [23, 60,110], [23, 250,300], - [24, 60,110], [24, 250,300], - [25, 60,110], [25, 250,300], - [26, 60,110], [26, 250,300], - [27, 60,106], [27, 254,300], - [28, 60,106], [28, 254,300], - [29, 60,106], [29, 254,300], - [30, 60,104], [30, 256,300], - [31, 60,104], [31, 256,300], - [32, 60,104], [32, 256,300], - [33, 60,104], [33, 256,300], - [34, 60,104], [34, 256,300], - [35, 60,100], [35, 260,300], - [36, 60,100], [36, 260,300], - [37, 60,100], [37, 260,300], - [38, 60,100], [38, 260,300], - [39, 60,96], [39, 264,300], - [40, 60,96], [40, 264,300], - [41, 60,96], [41, 264,300], - [42, 60,96], [42, 264,300], - [43, 60,96], [43, 264,300], - [44, 60,94], [44, 266,300], - [45, 60,94], [45, 266,300], - [46, 60,94], [46, 266,300], - [47, 60,94], [47, 266,300], + [0, 60, 148], [0, 212,360], + [1, 60, 144], [1, 216,360], + [2, 60, 142], [2, 218,360], + [3, 60, 140], [3, 220,360], + [4, 60, 138], [4, 222,360], + [5, 60, 138], [5, 222,360], + [6, 60, 134], [6, 226,360], + [7, 60, 134], [7, 226,360], + [8, 60, 132], [8, 228,360], + [9, 60, 132], [9, 228,360], + [10, 60, 130], [10, 230,360], + [11, 60, 120], [11, 240,360], + [12, 60, 118], [12, 242,360], + [13, 60, 118], [13, 242,360], + [14, 60, 118], [14, 242,360], + [15, 60, 118], [15, 242,360], + [16, 60, 118], [16, 242,360], + [17, 60, 118], [17, 242,360], + [18, 60, 116], [18, 244,360], + [19, 60, 116], [19, 244,360], + [20, 60, 112], [20, 248,360], + [21, 60, 112], [21, 248,360], + [22, 60, 110], [22, 250,360], + [23, 60, 110], [23, 250,360], + [24, 60, 110], [24, 250,360], + [25, 60, 110], [25, 250,360], + [26, 60, 110], [26, 250,360], + [27, 60, 106], [27, 254,360], + [28, 60, 106], [28, 254,360], + [29, 60, 106], [29, 254,360], + [30, 60, 104], [30, 256,360], + [31, 60, 104], [31, 256,360], + [32, 60, 104], [32, 256,360], + [33, 60, 104], [33, 256,360], + [34, 60, 104], [34, 256,360], + [35, 60, 100], [35, 260,360], + [36, 60, 100], [36, 260,360], + [37, 60, 100], [37, 260,360], + [38, 60, 100], [38, 260,360], + [39, 60, 96], [39, 264,360], + [40, 60, 96], [40, 264,360], + [41, 60, 96], [41, 264,360], + [42, 60, 96], [42, 264,360], + [43, 60, 96], [43, 264,360], + [44, 60, 94], [44, 266,360], + [45, 60, 94], [45, 266,360], + [46, 60, 94], [46, 266,360], + [47, 60, 94], [47, 266,360], [1, 0, 360], [2, 0, 360], [4, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml index 267e2258..b77ffde6 100644 --- a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml @@ -3,54 +3,54 @@ point_filters: | { "ring_section_filter": [ - [0, 60,148], [0, 212,300], - [1, 60,144], [1, 216,300], - [2, 60,142], [2, 218,300], - [3, 60,140], [3, 220,300], - [4, 60,138], [4, 222,300], - [5, 60,138], [5, 222,300], - [6, 60,134], [6, 226,300], - [7, 60,134], [7, 226,300], - [8, 60,132], [8, 228,300], - [9, 60,132], [9, 228,300], - [10, 60,130], [10, 230,300], - [11, 60,120], [11, 240,300], - [12, 60,118], [12, 242,300], - [13, 60,118], [13, 242,300], - [14, 60,118], [14, 242,300], - [15, 60,118], [15, 242,300], - [16, 60,118], [16, 242,300], - [17, 60,118], [17, 242,300], - [18, 60,116], [18, 244,300], - [19, 60,116], [19, 244,300], - [20, 60,112], [20, 248,300], - [21, 60,112], [21, 248,300], - [22, 60,110], [22, 250,300], - [23, 60,110], [23, 250,300], - [24, 60,110], [24, 250,300], - [25, 60,110], [25, 250,300], - [26, 60,110], [26, 250,300], - [27, 60,106], [27, 254,300], - [28, 60,106], [28, 254,300], - [29, 60,106], [29, 254,300], - [30, 60,104], [30, 256,300], - [31, 60,104], [31, 256,300], - [32, 60,104], [32, 256,300], - [33, 60,104], [33, 256,300], - [34, 60,104], [34, 256,300], - [35, 60,100], [35, 260,300], - [36, 60,100], [36, 260,300], - [37, 60,100], [37, 260,300], - [38, 60,100], [38, 260,300], - [39, 60,96], [39, 264,300], - [40, 60,96], [40, 264,300], - [41, 60,96], [41, 264,300], - [42, 60,96], [42, 264,300], - [43, 60,96], [43, 264,300], - [44, 60,94], [44, 266,300], - [45, 60,94], [45, 266,300], - [46, 60,94], [46, 266,300], - [47, 60,94], [47, 266,300], + [0, 60, 148], [0, 212,360], + [1, 60, 144], [1, 216,360], + [2, 60, 142], [2, 218,360], + [3, 60, 140], [3, 220,360], + [4, 60, 138], [4, 222,360], + [5, 60, 138], [5, 222,360], + [6, 60, 134], [6, 226,360], + [7, 60, 134], [7, 226,360], + [8, 60, 132], [8, 228,360], + [9, 60, 132], [9, 228,360], + [10, 60, 130], [10, 230,360], + [11, 60, 120], [11, 240,360], + [12, 60, 118], [12, 242,360], + [13, 60, 118], [13, 242,360], + [14, 60, 118], [14, 242,360], + [15, 60, 118], [15, 242,360], + [16, 60, 118], [16, 242,360], + [17, 60, 118], [17, 242,360], + [18, 60, 116], [18, 244,360], + [19, 60, 116], [19, 244,360], + [20, 60, 112], [20, 248,360], + [21, 60, 112], [21, 248,360], + [22, 60, 110], [22, 250,360], + [23, 60, 110], [23, 250,360], + [24, 60, 110], [24, 250,360], + [25, 60, 110], [25, 250,360], + [26, 60, 110], [26, 250,360], + [27, 60, 106], [27, 254,360], + [28, 60, 106], [28, 254,360], + [29, 60, 106], [29, 254,360], + [30, 60, 104], [30, 256,360], + [31, 60, 104], [31, 256,360], + [32, 60, 104], [32, 256,360], + [33, 60, 104], [33, 256,360], + [34, 60, 104], [34, 256,360], + [35, 60, 100], [35, 260,360], + [36, 60, 100], [36, 260,360], + [37, 60, 100], [37, 260,360], + [38, 60, 100], [38, 260,360], + [39, 60, 96], [39, 264,360], + [40, 60, 96], [40, 264,360], + [41, 60, 96], [41, 264,360], + [42, 60, 96], [42, 264,360], + [43, 60, 96], [43, 264,360], + [44, 60, 94], [44, 266,360], + [45, 60, 94], [45, 266,360], + [46, 60, 94], [46, 266,360], + [47, 60, 94], [47, 266,360], [0, 0, 360], [3, 0, 360], [6, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml index adcd8182..abbbc01c 100644 --- a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml @@ -3,111 +3,123 @@ point_filters: | { "ring_section_filter": [ - [0, 60,95],[0, 270,300], - [1, 60,95],[1, 270,300], - [2, 60,95],[2, 270,300], - [3, 60,95],[3, 270,300], - [4, 60,98],[4, 270,300], - [5, 60,98],[5, 270,300], - [6, 60,98], - [7, 60,98], - [8, 60,92], - [9, 60,92], - [10, 270,300], - [11, 270,300], - [12, 270,300], - [14, 265,300], - [15, 265,300], - [16, 265,300], - [17, 263,300], - [18, 263,300], - [19, 263,300], - [20, 263,300], - [21, 263,300], - [22, 264,300], - [23, 264,300], - [24, 264,300], - [30, 60,99],[30,264,300], - [31, 60,99],[31,264,300], - [32, 60,99],[32,264,300], - [33, 60,90], - [34, 60,90], - [35, 60,90], - [36, 60,94], - [37, 60,94], - [38, 60,94], - [39, 60,94], - [40, 60,94], - [41, 60,90], - [42, 60,90], - [43, 60,90], - [44, 60,100],[44, 262,300], - [45, 60,100],[45, 262,300], - [46, 60,100],[46, 262,300], - [51, 60,100], - [52, 60,100], - [53, 60,100], - [58, 60,99],[58, 269,300], - [59, 60,99],[59, 269,300], - [60, 60,99],[60, 269,300], - [65, 259.5,300], - [66, 259.5,300], - [67, 259.5,300], - [72, 265,300], - [73, 265,300], - [74, 265,300], - [75, 265,300], - [76, 265,300], - [77, 265,300], - [78, 60,98],[78, 262,300], - [79, 60,98],[79, 262,300], - [80, 60,98],[80, 262,300], - [81, 60,98],[81, 262,300], - [82, 60,98],[82, 262,300], - [83, 60,98],[83, 262,300], - [84, 60,98],[84, 262,300], - [85, 60,98],[85, 262,300], - [86, 60,98],[86, 262,300], - [87, 60,100],[87, 262,300], - [88, 60,100],[88, 262,300], - [89, 60,100],[89, 262,300], - [90, 262,300], - [91, 60,101], [91, 270,300], - [92, 60,101], [92, 270,300], - [93, 60,101], [93, 270,300], - [94, 60,102], [94, 270,300], - [95, 60,102],[95, 267,300], - [96, 60,102],[96, 267,300], - [97, 60,102],[97, 267,300], - [98, 60,95], - [99, 60,95], - [100, 60,95], - [101, 60,95], - [102, 60,95], - [103, 60,99], - [104, 60,97], - [105, 60,97], - [106, 60,97], - [107, 60,98], - [108, 60,98], - [109, 60,98], - [110, 60,93], - [111, 60,93],[111, 270,300], - [112, 60,93],[112, 270,300], - [113, 60,93],[113, 270,300], - [114, 60,98], - [115, 60,98], - [116, 60,98], - [117, 60,98], - [118, 60,100], - [119, 60,101],[119, 269,300], - [120, 60,101],[120, 269,300], - [121, 60,101],[121, 264,300], - [122, 60,101],[122, 264,300], - [123, 60,103], [123, 264,300], - [124, 60,103], [124, 258,300], - [125, 60,103], [125, 258,300], - [126, 60,103], [126, 258,300], - [127, 60,103], [127, 258,300] + [0, 0, 95],[0, 270, 360], + [1, 0, 95],[1, 270, 360], + [2, 0, 95],[2, 270, 360], + [3, 0, 95],[3, 270, 360], + [4, 0, 98],[4, 270, 360], + [5, 0, 98],[5, 270, 360], + [6, 0, 98],[6, 270, 360], + [7, 0, 98],[7, 270, 360], + [8, 0, 98],[8, 270, 360], + [9, 0, 99],[9, 270, 360], + [10, 0, 99],[10, 270, 360], + [11, 0, 99],[11, 270, 360], + [12, 0, 99],[12, 270, 360], + [13, 0, 104],[13, 270, 360], + [14, 0, 104],[14, 265, 360], + [15, 0, 104],[15, 265, 360], + [16, 0, 104],[16, 265, 360], + [17, 0, 104],[17, 263, 360], + [18, 0, 103],[18, 263, 360], + [19, 0, 103],[19, 263, 360], + [20, 0, 103],[20, 263, 360], + [21, 0, 103],[21, 263, 360], + [22, 0, 102],[22, 264, 360], + [23, 0, 102],[23, 264, 360], + [24, 0, 102],[24, 264, 360], + [25, 0, 102], + [29, 0, 106], + [30, 0, 106],[30,264, 360], + [31, 0, 106],[31,264, 360], + [32, 0, 106],[32,264, 360], + [33, 0, 106], + [34, 0, 90], + [35, 0, 90], + [36, 0, 102], + [37, 0, 102], + [38, 0, 102], + [39, 0, 102], + [40, 0, 102], + [41, 0, 90], + [42, 0, 90], + [43, 0, 101], + [44, 0, 101],[44, 262, 360], + [45, 0, 101],[45, 262, 360], + [46, 0, 101],[46, 262, 360], + [47, 0, 101],[47, 262, 360], + [50, 0, 106],[50, 268, 360], + [51, 0, 106],[51, 268, 360], + [52, 0, 106],[52, 268, 360], + [53, 0, 106],[53, 268, 360], + [54, 0, 106],[54, 268, 360], + [57, 0, 106],[57, 269, 360], + [58, 0, 106],[58, 269, 360], + [59, 0, 106],[59, 269, 360], + [60, 0, 106],[60, 269, 360], + [61, 0, 106],[61, 269, 360], + [62, 0, 100], + [63, 0, 100], + [64, 0, 100], + [65, 0, 100],[65, 259.5, 360], + [66, 0, 100],[66, 259.5, 360], + [67, 0, 100],[67, 259.5, 360], + [68, 0, 100], + [72, 265, 360], + [73, 265, 360], + [74, 265, 360], + [75, 265, 360], + [76, 265, 360], + [77, 265, 360], + [78, 0, 98],[78, 262, 360], + [79, 0, 98],[79, 262, 360], + [80, 0, 98],[80, 262, 360], + [81, 0, 98],[81, 262, 360], + [82, 0, 98],[82, 262, 360], + [83, 0, 98],[83, 262, 360], + [84, 0, 98],[84, 262, 360], + [85, 0, 98],[85, 262, 360], + [86, 0, 98],[86, 262, 360], + [87, 0, 100],[87, 262, 360], + [88, 0, 100],[88, 262, 360], + [89, 0, 100],[89, 262, 360], + [90, 0, 103], [90, 262, 360], + [91, 0, 103], [91, 266, 360], + [92, 0, 103], [92, 266, 360], + [93, 0, 103], [93, 266, 360], + [94, 0, 106], [94, 266, 360], + [95, 0, 106],[95, 266, 360], + [96, 0, 106],[96, 266, 360], + [97, 0, 106],[97, 266, 360], + [98, 0, 108],[98, 268, 360], + [99, 0, 108],[99, 268, 360], + [100, 0, 108],[100, 268, 360], + [101, 0, 108],[101, 268, 360], + [102, 0, 108],[102, 268, 360], + [103, 0, 108],[103, 268, 360], + [104, 0, 108],[104, 268, 360], + [105, 0, 108],[105, 268, 360], + [106, 0, 108],[106, 266, 360], + [107, 0, 106],[107, 266, 360], + [108, 0, 106],[108, 266, 360], + [109, 0, 106],[109, 266, 360], + [110, 0, 106],[110, 266, 360], + [111, 0, 106],[111, 266, 360], + [112, 0, 106],[112, 266, 360], + [113, 0, 106],[113, 266, 360], + [114, 0, 106],[114, 266, 360], + [115, 0, 106],[115, 266, 360], + [116, 0, 106],[116, 266, 360], + [117, 0, 107],[117, 266, 360], + [118, 0, 107],[118, 264, 360], + [119, 0, 107],[119, 264, 360], + [120, 0, 107],[120, 260, 360], + [121, 0, 107],[121, 260, 360], + [122, 0, 107],[122, 260, 360], + [123, 0, 110], [123, 260, 360], + [124, 0, 110], [124, 258, 360], + [125, 0, 110], [125, 258, 360], + [126, 0, 110], [126, 258, 360], + [127, 0, 110], [127, 258, 360] ] } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml index 550c33a9..331627ca 100644 --- a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml @@ -3,112 +3,124 @@ point_filters: | { "ring_section_filter": [ - [0, 60,95],[0, 270,300], - [1, 60,95],[1, 270,300], - [2, 60,95],[2, 270,300], - [3, 60,95],[3, 270,300], - [4, 60,98],[4, 270,300], - [5, 60,98],[5, 270,300], - [6, 60,98], - [7, 60,98], - [8, 60,92], - [9, 60,92], - [10, 270,300], - [11, 270,300], - [12, 270,300], - [14, 265,300], - [15, 265,300], - [16, 265,300], - [17, 263,300], - [18, 263,300], - [19, 263,300], - [20, 263,300], - [21, 263,300], - [22, 264,300], - [23, 264,300], - [24, 264,300], - [30, 60,99],[30,264,300], - [31, 60,99],[31,264,300], - [32, 60,99],[32,264,300], - [33, 60,90], - [34, 60,90], - [35, 60,90], - [36, 60,94], - [37, 60,94], - [38, 60,94], - [39, 60,94], - [40, 60,94], - [41, 60,90], - [42, 60,90], - [43, 60,90], - [44, 60,100],[44, 262,300], - [45, 60,100],[45, 262,300], - [46, 60,100],[46, 262,300], - [51, 60,100], - [52, 60,100], - [53, 60,100], - [58, 60,99],[58, 269,300], - [59, 60,99],[59, 269,300], - [60, 60,99],[60, 269,300], - [65, 259.5,300], - [66, 259.5,300], - [67, 259.5,300], - [72, 265,300], - [73, 265,300], - [74, 265,300], - [75, 265,300], - [76, 265,300], - [77, 265,300], - [78, 60,98],[78, 262,300], - [79, 60,98],[79, 262,300], - [80, 60,98],[80, 262,300], - [81, 60,98],[81, 262,300], - [82, 60,98],[82, 262,300], - [83, 60,98],[83, 262,300], - [84, 60,98],[84, 262,300], - [85, 60,98],[85, 262,300], - [86, 60,98],[86, 262,300], - [87, 60,100],[87, 262,300], - [88, 60,100],[88, 262,300], - [89, 60,100],[89, 262,300], - [90, 262,300], - [91, 60,101], [91, 270,300], - [92, 60,101], [92, 270,300], - [93, 60,101], [93, 270,300], - [94, 60,102], [94, 270,300], - [95, 60,102],[95, 267,300], - [96, 60,102],[96, 267,300], - [97, 60,102],[97, 267,300], - [98, 60,95], - [99, 60,95], - [100, 60,95], - [101, 60,95], - [102, 60,95], - [103, 60,99], - [104, 60,97], - [105, 60,97], - [106, 60,97], - [107, 60,98], - [108, 60,98], - [109, 60,98], - [110, 60,93], - [111, 60,93],[111, 270,300], - [112, 60,93],[112, 270,300], - [113, 60,93],[113, 270,300], - [114, 60,98], - [115, 60,98], - [116, 60,98], - [117, 60,98], - [118, 60,100], - [119, 60,101],[119, 269,300], - [120, 60,101],[120, 269,300], - [121, 60,101],[121, 264,300], - [122, 60,101],[122, 264,300], - [123, 60,103], [123, 264,300], - [124, 60,103], [124, 258,300], - [125, 60,103], [125, 258,300], - [126, 60,103], [126, 258,300], - [127, 60,103], [127, 258,300], + [0, 0, 95],[0, 270, 360], + [1, 0, 95],[1, 270, 360], + [2, 0, 95],[2, 270, 360], + [3, 0, 95],[3, 270, 360], + [4, 0, 98],[4, 270, 360], + [5, 0, 98],[5, 270, 360], + [6, 0, 98],[6, 270, 360], + [7, 0, 98],[7, 270, 360], + [8, 0, 98],[8, 270, 360], + [9, 0, 99],[9, 270, 360], + [10, 0, 99],[10, 270, 360], + [11, 0, 99],[11, 270, 360], + [12, 0, 99],[12, 270, 360], + [13, 0, 104],[13, 270, 360], + [14, 0, 104],[14, 265, 360], + [15, 0, 104],[15, 265, 360], + [16, 0, 104],[16, 265, 360], + [17, 0, 104],[17, 263, 360], + [18, 0, 103],[18, 263, 360], + [19, 0, 103],[19, 263, 360], + [20, 0, 103],[20, 263, 360], + [21, 0, 103],[21, 263, 360], + [22, 0, 102],[22, 264, 360], + [23, 0, 102],[23, 264, 360], + [24, 0, 102],[24, 264, 360], + [25, 0, 102], + [29, 0, 106], + [30, 0, 106],[30,264, 360], + [31, 0, 106],[31,264, 360], + [32, 0, 106],[32,264, 360], + [33, 0, 106], + [34, 0, 90], + [35, 0, 90], + [36, 0, 102], + [37, 0, 102], + [38, 0, 102], + [39, 0, 102], + [40, 0, 102], + [41, 0, 90], + [42, 0, 90], + [43, 0, 101], + [44, 0, 101],[44, 262, 360], + [45, 0, 101],[45, 262, 360], + [46, 0, 101],[46, 262, 360], + [47, 0, 101],[47, 262, 360], + [50, 0, 106],[50, 268, 360], + [51, 0, 106],[51, 268, 360], + [52, 0, 106],[52, 268, 360], + [53, 0, 106],[53, 268, 360], + [54, 0, 106],[54, 268, 360], + [57, 0, 106],[57, 269, 360], + [58, 0, 106],[58, 269, 360], + [59, 0, 106],[59, 269, 360], + [60, 0, 106],[60, 269, 360], + [61, 0, 106],[61, 269, 360], + [62, 0, 100], + [63, 0, 100], + [64, 0, 100], + [65, 0, 100],[65, 259.5, 360], + [66, 0, 100],[66, 259.5, 360], + [67, 0, 100],[67, 259.5, 360], + [68, 0, 100], + [72, 265, 360], + [73, 265, 360], + [74, 265, 360], + [75, 265, 360], + [76, 265, 360], + [77, 265, 360], + [78, 0, 98],[78, 262, 360], + [79, 0, 98],[79, 262, 360], + [80, 0, 98],[80, 262, 360], + [81, 0, 98],[81, 262, 360], + [82, 0, 98],[82, 262, 360], + [83, 0, 98],[83, 262, 360], + [84, 0, 98],[84, 262, 360], + [85, 0, 98],[85, 262, 360], + [86, 0, 98],[86, 262, 360], + [87, 0, 100],[87, 262, 360], + [88, 0, 100],[88, 262, 360], + [89, 0, 100],[89, 262, 360], + [90, 0, 103], [90, 262, 360], + [91, 0, 103], [91, 266, 360], + [92, 0, 103], [92, 266, 360], + [93, 0, 103], [93, 266, 360], + [94, 0, 106], [94, 266, 360], + [95, 0, 106],[95, 266, 360], + [96, 0, 106],[96, 266, 360], + [97, 0, 106],[97, 266, 360], + [98, 0, 108],[98, 268, 360], + [99, 0, 108],[99, 268, 360], + [100, 0, 108],[100, 268, 360], + [101, 0, 108],[101, 268, 360], + [102, 0, 108],[102, 268, 360], + [103, 0, 108],[103, 268, 360], + [104, 0, 108],[104, 268, 360], + [105, 0, 108],[105, 268, 360], + [106, 0, 108],[106, 266, 360], + [107, 0, 106],[107, 266, 360], + [108, 0, 106],[108, 266, 360], + [109, 0, 106],[109, 266, 360], + [110, 0, 106],[110, 266, 360], + [111, 0, 106],[111, 266, 360], + [112, 0, 106],[112, 266, 360], + [113, 0, 106],[113, 266, 360], + [114, 0, 106],[114, 266, 360], + [115, 0, 106],[115, 266, 360], + [116, 0, 106],[116, 266, 360], + [117, 0, 107],[117, 266, 360], + [118, 0, 107],[118, 264, 360], + [119, 0, 107],[119, 264, 360], + [120, 0, 107],[120, 260, 360], + [121, 0, 107],[121, 260, 360], + [122, 0, 107],[122, 260, 360], + [123, 0, 110], [123, 260, 360], + [124, 0, 110], [124, 258, 360], + [125, 0, 110], [125, 258, 360], + [126, 0, 110], [126, 258, 360], + [127, 0, 110], [127, 258, 360], [1, 0, 360], [2, 0, 360], [4, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml index e3f7d468..a31c771c 100644 --- a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml @@ -3,112 +3,124 @@ point_filters: | { "ring_section_filter": [ - [0, 60,95],[0, 270,300], - [1, 60,95],[1, 270,300], - [2, 60,95],[2, 270,300], - [3, 60,95],[3, 270,300], - [4, 60,98],[4, 270,300], - [5, 60,98],[5, 270,300], - [6, 60,98], - [7, 60,98], - [8, 60,92], - [9, 60,92], - [10, 270,300], - [11, 270,300], - [12, 270,300], - [14, 265,300], - [15, 265,300], - [16, 265,300], - [17, 263,300], - [18, 263,300], - [19, 263,300], - [20, 263,300], - [21, 263,300], - [22, 264,300], - [23, 264,300], - [24, 264,300], - [30, 60,99],[30,264,300], - [31, 60,99],[31,264,300], - [32, 60,99],[32,264,300], - [33, 60,90], - [34, 60,90], - [35, 60,90], - [36, 60,94], - [37, 60,94], - [38, 60,94], - [39, 60,94], - [40, 60,94], - [41, 60,90], - [42, 60,90], - [43, 60,90], - [44, 60,100],[44, 262,300], - [45, 60,100],[45, 262,300], - [46, 60,100],[46, 262,300], - [51, 60,100], - [52, 60,100], - [53, 60,100], - [58, 60,99],[58, 269,300], - [59, 60,99],[59, 269,300], - [60, 60,99],[60, 269,300], - [65, 259.5,300], - [66, 259.5,300], - [67, 259.5,300], - [72, 265,300], - [73, 265,300], - [74, 265,300], - [75, 265,300], - [76, 265,300], - [77, 265,300], - [78, 60,98],[78, 262,300], - [79, 60,98],[79, 262,300], - [80, 60,98],[80, 262,300], - [81, 60,98],[81, 262,300], - [82, 60,98],[82, 262,300], - [83, 60,98],[83, 262,300], - [84, 60,98],[84, 262,300], - [85, 60,98],[85, 262,300], - [86, 60,98],[86, 262,300], - [87, 60,100],[87, 262,300], - [88, 60,100],[88, 262,300], - [89, 60,100],[89, 262,300], - [90, 262,300], - [91, 60,101], [91, 270,300], - [92, 60,101], [92, 270,300], - [93, 60,101], [93, 270,300], - [94, 60,102], [94, 270,300], - [95, 60,102],[95, 267,300], - [96, 60,102],[96, 267,300], - [97, 60,102],[97, 267,300], - [98, 60,95], - [99, 60,95], - [100, 60,95], - [101, 60,95], - [102, 60,95], - [103, 60,99], - [104, 60,97], - [105, 60,97], - [106, 60,97], - [107, 60,98], - [108, 60,98], - [109, 60,98], - [110, 60,93], - [111, 60,93],[111, 270,300], - [112, 60,93],[112, 270,300], - [113, 60,93],[113, 270,300], - [114, 60,98], - [115, 60,98], - [116, 60,98], - [117, 60,98], - [118, 60,100], - [119, 60,101],[119, 269,300], - [120, 60,101],[120, 269,300], - [121, 60,101],[121, 264,300], - [122, 60,101],[122, 264,300], - [123, 60,103], [123, 264,300], - [124, 60,103], [124, 258,300], - [125, 60,103], [125, 258,300], - [126, 60,103], [126, 258,300], - [127, 60,103], [127, 258,300], + [0, 0, 95],[0, 270, 360], + [1, 0, 95],[1, 270, 360], + [2, 0, 95],[2, 270, 360], + [3, 0, 95],[3, 270, 360], + [4, 0, 98],[4, 270, 360], + [5, 0, 98],[5, 270, 360], + [6, 0, 98],[6, 270, 360], + [7, 0, 98],[7, 270, 360], + [8, 0, 98],[8, 270, 360], + [9, 0, 99],[9, 270, 360], + [10, 0, 99],[10, 270, 360], + [11, 0, 99],[11, 270, 360], + [12, 0, 99],[12, 270, 360], + [13, 0, 104],[13, 270, 360], + [14, 0, 104],[14, 265, 360], + [15, 0, 104],[15, 265, 360], + [16, 0, 104],[16, 265, 360], + [17, 0, 104],[17, 263, 360], + [18, 0, 103],[18, 263, 360], + [19, 0, 103],[19, 263, 360], + [20, 0, 103],[20, 263, 360], + [21, 0, 103],[21, 263, 360], + [22, 0, 102],[22, 264, 360], + [23, 0, 102],[23, 264, 360], + [24, 0, 102],[24, 264, 360], + [25, 0, 102], + [29, 0, 106], + [30, 0, 106],[30,264, 360], + [31, 0, 106],[31,264, 360], + [32, 0, 106],[32,264, 360], + [33, 0, 106], + [34, 0, 90], + [35, 0, 90], + [36, 0, 102], + [37, 0, 102], + [38, 0, 102], + [39, 0, 102], + [40, 0, 102], + [41, 0, 90], + [42, 0, 90], + [43, 0, 101], + [44, 0, 101],[44, 262, 360], + [45, 0, 101],[45, 262, 360], + [46, 0, 101],[46, 262, 360], + [47, 0, 101],[47, 262, 360], + [50, 0, 106],[50, 268, 360], + [51, 0, 106],[51, 268, 360], + [52, 0, 106],[52, 268, 360], + [53, 0, 106],[53, 268, 360], + [54, 0, 106],[54, 268, 360], + [57, 0, 106],[57, 269, 360], + [58, 0, 106],[58, 269, 360], + [59, 0, 106],[59, 269, 360], + [60, 0, 106],[60, 269, 360], + [61, 0, 106],[61, 269, 360], + [62, 0, 100], + [63, 0, 100], + [64, 0, 100], + [65, 0, 100],[65, 259.5, 360], + [66, 0, 100],[66, 259.5, 360], + [67, 0, 100],[67, 259.5, 360], + [68, 0, 100], + [72, 265, 360], + [73, 265, 360], + [74, 265, 360], + [75, 265, 360], + [76, 265, 360], + [77, 265, 360], + [78, 0, 98],[78, 262, 360], + [79, 0, 98],[79, 262, 360], + [80, 0, 98],[80, 262, 360], + [81, 0, 98],[81, 262, 360], + [82, 0, 98],[82, 262, 360], + [83, 0, 98],[83, 262, 360], + [84, 0, 98],[84, 262, 360], + [85, 0, 98],[85, 262, 360], + [86, 0, 98],[86, 262, 360], + [87, 0, 100],[87, 262, 360], + [88, 0, 100],[88, 262, 360], + [89, 0, 100],[89, 262, 360], + [90, 0, 103], [90, 262, 360], + [91, 0, 103], [91, 266, 360], + [92, 0, 103], [92, 266, 360], + [93, 0, 103], [93, 266, 360], + [94, 0, 106], [94, 266, 360], + [95, 0, 106],[95, 266, 360], + [96, 0, 106],[96, 266, 360], + [97, 0, 106],[97, 266, 360], + [98, 0, 108],[98, 268, 360], + [99, 0, 108],[99, 268, 360], + [100, 0, 108],[100, 268, 360], + [101, 0, 108],[101, 268, 360], + [102, 0, 108],[102, 268, 360], + [103, 0, 108],[103, 268, 360], + [104, 0, 108],[104, 268, 360], + [105, 0, 108],[105, 268, 360], + [106, 0, 108],[106, 266, 360], + [107, 0, 106],[107, 266, 360], + [108, 0, 106],[108, 266, 360], + [109, 0, 106],[109, 266, 360], + [110, 0, 106],[110, 266, 360], + [111, 0, 106],[111, 266, 360], + [112, 0, 106],[112, 266, 360], + [113, 0, 106],[113, 266, 360], + [114, 0, 106],[114, 266, 360], + [115, 0, 106],[115, 266, 360], + [116, 0, 106],[116, 266, 360], + [117, 0, 107],[117, 266, 360], + [118, 0, 107],[118, 264, 360], + [119, 0, 107],[119, 264, 360], + [120, 0, 107],[120, 260, 360], + [121, 0, 107],[121, 260, 360], + [122, 0, 107],[122, 260, 360], + [123, 0, 110], [123, 260, 360], + [124, 0, 110], [124, 258, 360], + [125, 0, 110], [125, 258, 360], + [126, 0, 110], [126, 258, 360], + [127, 0, 110], [127, 258, 360], [0, 0, 360], [3, 0, 360], [6, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml index 95e10b77..e47031b9 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml @@ -6,55 +6,55 @@ [0, 0, 360], [1, 0, 360], [2, 0, 360], - [3, 60, 165],[3, 190, 300], - [4, 60, 158],[4, 203, 300], - [5, 60, 157],[5, 205, 300], - [6, 60, 156],[6, 211, 300], - [7, 60, 154],[7, 214, 300], - [8, 60, 152],[8, 217, 300], - [9, 60, 152],[9, 220, 300], - [10, 60, 151],[10, 221, 300], - [11, 60, 151],[11, 226, 300], - [12, 60, 151],[12, 229, 300], - [13, 60, 151],[13, 231, 300], - [14, 60, 150],[14, 232, 300], - [15, 60, 150],[15, 233, 300], - [16, 60, 150],[16, 234, 300], - [17, 60, 140],[17, 235, 300], - [18, 60, 135],[18, 235, 300], - [19, 60, 126],[19, 236, 300], - [20, 102, 126],[20, 240, 300], - [21, 102, 125],[21, 241, 300], - [22, 105, 125],[22, 241, 300], - [23, 105, 124],[23, 244, 300], - [24, 105, 123],[24, 245, 300], - [25, 106, 122],[25, 246, 300], - [26, 107, 121],[26, 247, 300], - [27, 108, 120],[27, 248, 300], - [28, 109, 119],[28, 248, 300], - [29, 110, 118],[29, 250, 300], - [30, 111, 117],[30, 250, 300], - [31, 112.5, 116],[31, 251, 300], - [32, 251, 300], - [33, 252, 300], - [34, 252, 300], - [35, 253, 300], - [36, 253, 300], - [37, 254, 300], - [38, 254, 300], - [39, 255, 300], - [40, 255, 300], - [41, 256, 300], - [42, 256, 300], - [43, 257, 300], - [44, 258, 300], - [45, 259, 300], - [46, 264, 300], - [47, 265, 300], - [48, 265, 300], - [49, 266, 300], - [50, 267, 300], - [51, 267, 300], - [52, 268, 300] + [3, 0, 165],[3, 190, 360], + [4, 0, 158],[4, 203, 360], + [5, 0, 157],[5, 205, 360], + [6, 0, 156],[6, 211, 360], + [7, 0, 154],[7, 214, 360], + [8, 0, 152],[8, 217, 360], + [9, 0, 152],[9, 220, 360], + [10, 0, 151],[10, 221, 360], + [11, 0, 151],[11, 226, 360], + [12, 0, 151],[12, 229, 360], + [13, 0, 151],[13, 231, 360], + [14, 0, 150],[14, 232, 360], + [15, 0, 150],[15, 233, 360], + [16, 0, 150],[16, 234, 360], + [17, 0, 145],[17, 235, 360], + [18, 0, 145],[18, 235, 360], + [19, 0, 135],[19, 236, 360], + [20, 102, 130],[20, 240, 360], + [21, 102, 125],[21, 241, 360], + [22, 105, 125],[22, 241, 360], + [23, 105, 124],[23, 244, 360], + [24, 105, 123],[24, 245, 360], + [25, 106, 122],[25, 246, 360], + [26, 107, 121],[26, 247, 360], + [27, 108, 120],[27, 248, 360], + [28, 109, 119],[28, 248, 360], + [29, 110, 118],[29, 250, 360], + [30, 111, 117],[30, 250, 360], + [31, 112.5, 116],[31, 251, 360], + [32, 251, 360], + [33, 252, 360], + [34, 252, 360], + [35, 253, 360], + [36, 253, 360], + [37, 254, 360], + [38, 254, 360], + [39, 255, 360], + [40, 255, 360], + [41, 256, 360], + [42, 256, 360], + [43, 257, 360], + [44, 258, 360], + [45, 259, 360], + [46, 264, 360], + [47, 265, 360], + [48, 265, 360], + [49, 266, 360], + [50, 267, 360], + [51, 267, 360], + [52, 268, 360] ] } diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml index 351c5363..2a972111 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml @@ -6,56 +6,56 @@ [0, 0, 360], [1, 0, 360], [2, 0, 360], - [3, 60, 165],[3, 190, 300], - [4, 60, 158],[4, 203, 300], - [5, 60, 157],[5, 205, 300], - [6, 60, 156],[6, 211, 300], - [7, 60, 154],[7, 214, 300], - [8, 60, 152],[8, 217, 300], - [9, 60, 152],[9, 220, 300], - [10, 60, 151],[10, 221, 300], - [11, 60, 151],[11, 226, 300], - [12, 60, 151],[12, 229, 300], - [13, 60, 151],[13, 231, 300], - [14, 60, 150],[14, 232, 300], - [15, 60, 150],[15, 233, 300], - [16, 60, 150],[16, 234, 300], - [17, 60, 140],[17, 235, 300], - [18, 60, 135],[18, 235, 300], - [19, 60, 126],[19, 236, 300], - [20, 103, 126],[20, 240, 300], - [21, 105, 125],[21, 241, 300], - [22, 105, 125],[22, 241, 300], - [23, 105, 124],[23, 244, 300], - [24, 105, 123],[24, 245, 300], - [25, 106, 122],[25, 246, 300], - [26, 107, 121],[26, 247, 300], - [27, 108, 120],[27, 248, 300], - [28, 109, 119],[28, 248, 300], - [29, 110, 118],[29, 250, 300], - [30, 111, 117],[30, 250, 300], - [31, 112.5, 116],[31, 251, 300], - [32, 251, 300], - [33, 252, 300], - [34, 252, 300], - [35, 253, 300], - [36, 253, 300], - [37, 254, 300], - [38, 254, 300], - [39, 255, 300], - [40, 255, 300], - [41, 256, 300], - [42, 256, 300], - [43, 257, 300], - [44, 258, 300], - [45, 259, 300], - [46, 264, 300], - [47, 265, 300], - [48, 265, 300], - [49, 266, 300], - [50, 267, 300], - [51, 267, 300], - [52, 268, 300], + [3, 0, 165],[3, 190, 360], + [4, 0, 158],[4, 203, 360], + [5, 0, 157],[5, 205, 360], + [6, 0, 156],[6, 211, 360], + [7, 0, 154],[7, 214, 360], + [8, 0, 152],[8, 217, 360], + [9, 0, 152],[9, 220, 360], + [10, 0, 151],[10, 221, 360], + [11, 0, 151],[11, 226, 360], + [12, 0, 151],[12, 229, 360], + [13, 0, 151],[13, 231, 360], + [14, 0, 150],[14, 232, 360], + [15, 0, 150],[15, 233, 360], + [16, 0, 150],[16, 234, 360], + [17, 0, 145],[17, 235, 360], + [18, 0, 145],[18, 235, 360], + [19, 0, 135],[19, 236, 360], + [20, 102, 130],[20, 240, 360], + [21, 105, 125],[21, 241, 360], + [22, 105, 125],[22, 241, 360], + [23, 105, 124],[23, 244, 360], + [24, 105, 123],[24, 245, 360], + [25, 106, 122],[25, 246, 360], + [26, 107, 121],[26, 247, 360], + [27, 108, 120],[27, 248, 360], + [28, 109, 119],[28, 248, 360], + [29, 110, 118],[29, 250, 360], + [30, 111, 117],[30, 250, 360], + [31, 112.5, 116],[31, 251, 360], + [32, 251, 360], + [33, 252, 360], + [34, 252, 360], + [35, 253, 360], + [36, 253, 360], + [37, 254, 360], + [38, 254, 360], + [39, 255, 360], + [40, 255, 360], + [41, 256, 360], + [42, 256, 360], + [43, 257, 360], + [44, 258, 360], + [45, 259, 360], + [46, 264, 360], + [47, 265, 360], + [48, 265, 360], + [49, 266, 360], + [50, 267, 360], + [51, 267, 360], + [52, 268, 360], [1, 0, 360], [2, 0, 360], [4, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml index 6e82a756..c8b8fbbf 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml @@ -6,56 +6,56 @@ [0, 0, 360], [1, 0, 360], [2, 0, 360], - [3, 60, 165],[3, 190, 300], - [4, 60, 158],[4, 203, 300], - [5, 60, 157],[5, 205, 300], - [6, 60, 156],[6, 211, 300], - [7, 60, 154],[7, 214, 300], - [8, 60, 152],[8, 217, 300], - [9, 60, 152],[9, 220, 300], - [10, 60, 151],[10, 221, 300], - [11, 60, 151],[11, 226, 300], - [12, 60, 151],[12, 229, 300], - [13, 60, 151],[13, 231, 300], - [14, 60, 150],[14, 232, 300], - [15, 60, 150],[15, 233, 300], - [16, 60, 150],[16, 234, 300], - [17, 60, 140],[17, 235, 300], - [18, 60, 135],[18, 235, 300], - [19, 60, 126],[19, 236, 300], - [20, 103, 126],[20, 240, 300], - [21, 105, 125],[21, 241, 300], - [22, 105, 125],[22, 241, 300], - [23, 105, 124],[23, 244, 300], - [24, 105, 123],[24, 245, 300], - [25, 106, 122],[25, 246, 300], - [26, 107, 121],[26, 247, 300], - [27, 108, 120],[27, 248, 300], - [28, 109, 119],[28, 248, 300], - [29, 110, 118],[29, 250, 300], - [30, 111, 117],[30, 250, 300], - [31, 112.5, 116],[31, 251, 300], - [32, 251, 300], - [33, 252, 300], - [34, 252, 300], - [35, 253, 300], - [36, 253, 300], - [37, 254, 300], - [38, 254, 300], - [39, 255, 300], - [40, 255, 300], - [41, 256, 300], - [42, 256, 300], - [43, 257, 300], - [44, 258, 300], - [45, 259, 300], - [46, 264, 300], - [47, 265, 300], - [48, 265, 300], - [49, 266, 300], - [50, 267, 300], - [51, 267, 300], - [52, 268, 300], + [3, 0, 165],[3, 190, 360], + [4, 0, 158],[4, 203, 360], + [5, 0, 157],[5, 205, 360], + [6, 0, 156],[6, 211, 360], + [7, 0, 154],[7, 214, 360], + [8, 0, 152],[8, 217, 360], + [9, 0, 152],[9, 220, 360], + [10, 0, 151],[10, 221, 360], + [11, 0, 151],[11, 226, 360], + [12, 0, 151],[12, 229, 360], + [13, 0, 151],[13, 231, 360], + [14, 0, 150],[14, 232, 360], + [15, 0, 150],[15, 233, 360], + [16, 0, 150],[16, 234, 360], + [17, 0, 145],[17, 235, 360], + [18, 0, 145],[18, 235, 360], + [19, 0, 135],[19, 236, 360], + [20, 102, 130],[20, 240, 360], + [21, 105, 125],[21, 241, 360], + [22, 105, 125],[22, 241, 360], + [23, 105, 124],[23, 244, 360], + [24, 105, 123],[24, 245, 360], + [25, 106, 122],[25, 246, 360], + [26, 107, 121],[26, 247, 360], + [27, 108, 120],[27, 248, 360], + [28, 109, 119],[28, 248, 360], + [29, 110, 118],[29, 250, 360], + [30, 111, 117],[30, 250, 360], + [31, 112.5, 116],[31, 251, 360], + [32, 251, 360], + [33, 252, 360], + [34, 252, 360], + [35, 253, 360], + [36, 253, 360], + [37, 254, 360], + [38, 254, 360], + [39, 255, 360], + [40, 255, 360], + [41, 256, 360], + [42, 256, 360], + [43, 257, 360], + [44, 258, 360], + [45, 259, 360], + [46, 264, 360], + [47, 265, 360], + [48, 265, 360], + [49, 266, 360], + [50, 267, 360], + [51, 267, 360], + [52, 268, 360], [0, 0, 360], [3, 0, 360], [6, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml index a0fc854a..5e5fb040 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml @@ -7,6 +7,12 @@ [1, 0, 15], [2, 0, 15], [3, 0, 15], + [17, 0, 10], + [18, 0, 10], + [19, 0, 10], + [20, 0, 10], + [21, 0, 10], + [22, 0, 10], [23, 0, 10], [24, 0, 10], [25, 0, 10], @@ -51,43 +57,43 @@ [64, 0, 35], [65, 0, 35], [66, 0, 40], - [67, 0, 40], - [68, 0, 42], - [69, 0, 42], - [70, 0, 42], - [71, 0, 44], - [72, 0, 44], - [73, 0, 44], - [74, 0, 44], - [75, 0, 44], - [76, 0, 45], - [77, 0, 45], - [78, 0, 45], - [79, 0, 45], - [80, 0, 45], - [81, 0, 45], - [82, 0, 45], - [83, 0, 45], - [84, 0, 45], - [85, 0, 45], - [86, 0, 45], - [87, 0, 45], - [88, 0, 45], - [89, 0, 45], - [90, 0, 45], - [91, 0, 45], - [92, 0, 45], - [93, 0, 44], - [94, 0, 44], - [95, 0, 44], - [96, 0, 44], - [97, 0, 44], - [98, 0, 44], - [99, 0, 44], - [100, 0, 46], - [101, 0, 46], - [102, 0, 46], - [103, 0, 44], + [67, 0, 40],[67, 269, 360], + [68, 0, 42],[68, 269, 360], + [69, 0, 42],[69, 269, 360], + [70, 0, 42],[70, 269, 360], + [71, 0, 44],[71, 269, 360], + [72, 0, 44],[72, 269, 360], + [73, 0, 44],[73, 269, 360], + [74, 0, 44],[74, 269, 360], + [75, 0, 44],[75, 269, 360], + [76, 0, 45],[76, 269, 360], + [77, 0, 45],[77, 269, 360], + [78, 0, 45],[78, 269, 360], + [79, 0, 45],[79, 269, 360], + [80, 0, 45],[80, 269, 360], + [81, 0, 45],[81, 269, 360], + [82, 0, 45],[82, 269, 360], + [83, 0, 45],[83, 269, 360], + [84, 0, 45],[84, 269, 360], + [85, 0, 45],[85, 269, 360], + [86, 0, 45],[86, 269, 360], + [87, 0, 45],[87, 269, 360], + [88, 0, 45],[88, 269, 360], + [89, 0, 45],[89, 269, 360], + [90, 0, 45],[90, 269, 360], + [91, 0, 45],[91, 269, 360], + [92, 0, 45],[92, 269, 360], + [93, 0, 44],[93, 269, 360], + [94, 0, 44],[94, 269, 360], + [95, 0, 44],[95, 269, 360], + [96, 0, 44],[96, 269, 360], + [97, 0, 44],[97, 269, 360], + [98, 0, 44],[98, 269, 360], + [99, 0, 44],[99, 269, 360], + [100, 0, 46],[100, 269, 360], + [101, 0, 46],[101, 269, 360], + [102, 0, 46],[102, 269, 360], + [103, 0, 44],[103, 269, 360], [104, 0, 44],[104, 269, 360], [105, 0, 44],[105, 269, 360], [106, 0, 44],[106, 269, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml index 35f6a53f..e903fa97 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml @@ -7,6 +7,12 @@ [1, 0, 15], [2, 0, 15], [3, 0, 15], + [17, 0, 10], + [18, 0, 10], + [19, 0, 10], + [20, 0, 10], + [21, 0, 10], + [22, 0, 10], [23, 0, 10], [24, 0, 10], [25, 0, 10], @@ -51,43 +57,43 @@ [64, 0, 35], [65, 0, 35], [66, 0, 40], - [67, 0, 40], - [68, 0, 42], - [69, 0, 42], - [70, 0, 42], - [71, 0, 44], - [72, 0, 44], - [73, 0, 44], - [74, 0, 44], - [75, 0, 44], - [76, 0, 45], - [77, 0, 45], - [78, 0, 45], - [79, 0, 45], - [80, 0, 45], - [81, 0, 45], - [82, 0, 45], - [83, 0, 45], - [84, 0, 45], - [85, 0, 45], - [86, 0, 45], - [87, 0, 45], - [88, 0, 45], - [89, 0, 45], - [90, 0, 45], - [91, 0, 45], - [92, 0, 45], - [93, 0, 44], - [94, 0, 44], - [95, 0, 44], - [96, 0, 44], - [97, 0, 44], - [98, 0, 44], - [99, 0, 44], - [100, 0, 46], - [101, 0, 46], - [102, 0, 46], - [103, 0, 44], + [67, 0, 40],[67, 269, 360], + [68, 0, 42],[68, 269, 360], + [69, 0, 42],[69, 269, 360], + [70, 0, 42],[70, 269, 360], + [71, 0, 44],[71, 269, 360], + [72, 0, 44],[72, 269, 360], + [73, 0, 44],[73, 269, 360], + [74, 0, 44],[74, 269, 360], + [75, 0, 44],[75, 269, 360], + [76, 0, 45],[76, 269, 360], + [77, 0, 45],[77, 269, 360], + [78, 0, 45],[78, 269, 360], + [79, 0, 45],[79, 269, 360], + [80, 0, 45],[80, 269, 360], + [81, 0, 45],[81, 269, 360], + [82, 0, 45],[82, 269, 360], + [83, 0, 45],[83, 269, 360], + [84, 0, 45],[84, 269, 360], + [85, 0, 45],[85, 269, 360], + [86, 0, 45],[86, 269, 360], + [87, 0, 45],[87, 269, 360], + [88, 0, 45],[88, 269, 360], + [89, 0, 45],[89, 269, 360], + [90, 0, 45],[90, 269, 360], + [91, 0, 45],[91, 269, 360], + [92, 0, 45],[92, 269, 360], + [93, 0, 44],[93, 269, 360], + [94, 0, 44],[94, 269, 360], + [95, 0, 44],[95, 269, 360], + [96, 0, 44],[96, 269, 360], + [97, 0, 44],[97, 269, 360], + [98, 0, 44],[98, 269, 360], + [99, 0, 44],[99, 269, 360], + [100, 0, 46],[100, 269, 360], + [101, 0, 46],[101, 269, 360], + [102, 0, 46],[102, 269, 360], + [103, 0, 44],[103, 269, 360], [104, 0, 44],[104, 269, 360], [105, 0, 44],[105, 269, 360], [106, 0, 44],[106, 269, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml index b1de13ab..015d8cfc 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml @@ -7,6 +7,12 @@ [1, 0, 15], [2, 0, 15], [3, 0, 15], + [17, 0, 10], + [18, 0, 10], + [19, 0, 10], + [20, 0, 10], + [21, 0, 10], + [22, 0, 10], [23, 0, 10], [24, 0, 10], [25, 0, 10], @@ -51,43 +57,43 @@ [64, 0, 35], [65, 0, 35], [66, 0, 40], - [67, 0, 40], - [68, 0, 42], - [69, 0, 42], - [70, 0, 42], - [71, 0, 44], - [72, 0, 44], - [73, 0, 44], - [74, 0, 44], - [75, 0, 44], - [76, 0, 45], - [77, 0, 45], - [78, 0, 45], - [79, 0, 45], - [80, 0, 45], - [81, 0, 45], - [82, 0, 45], - [83, 0, 45], - [84, 0, 45], - [85, 0, 45], - [86, 0, 45], - [87, 0, 45], - [88, 0, 45], - [89, 0, 45], - [90, 0, 45], - [91, 0, 45], - [92, 0, 45], - [93, 0, 44], - [94, 0, 44], - [95, 0, 44], - [96, 0, 44], - [97, 0, 44], - [98, 0, 44], - [99, 0, 44], - [100, 0, 46], - [101, 0, 46], - [102, 0, 46], - [103, 0, 44], + [67, 0, 40],[67, 269, 360], + [68, 0, 42],[68, 269, 360], + [69, 0, 42],[69, 269, 360], + [70, 0, 42],[70, 269, 360], + [71, 0, 44],[71, 269, 360], + [72, 0, 44],[72, 269, 360], + [73, 0, 44],[73, 269, 360], + [74, 0, 44],[74, 269, 360], + [75, 0, 44],[75, 269, 360], + [76, 0, 45],[76, 269, 360], + [77, 0, 45],[77, 269, 360], + [78, 0, 45],[78, 269, 360], + [79, 0, 45],[79, 269, 360], + [80, 0, 45],[80, 269, 360], + [81, 0, 45],[81, 269, 360], + [82, 0, 45],[82, 269, 360], + [83, 0, 45],[83, 269, 360], + [84, 0, 45],[84, 269, 360], + [85, 0, 45],[85, 269, 360], + [86, 0, 45],[86, 269, 360], + [87, 0, 45],[87, 269, 360], + [88, 0, 45],[88, 269, 360], + [89, 0, 45],[89, 269, 360], + [90, 0, 45],[90, 269, 360], + [91, 0, 45],[91, 269, 360], + [92, 0, 45],[92, 269, 360], + [93, 0, 44],[93, 269, 360], + [94, 0, 44],[94, 269, 360], + [95, 0, 44],[95, 269, 360], + [96, 0, 44],[96, 269, 360], + [97, 0, 44],[97, 269, 360], + [98, 0, 44],[98, 269, 360], + [99, 0, 44],[99, 269, 360], + [100, 0, 46],[100, 269, 360], + [101, 0, 46],[101, 269, 360], + [102, 0, 46],[102, 269, 360], + [103, 0, 44],[103, 269, 360], [104, 0, 44],[104, 269, 360], [105, 0, 44],[105, 269, 360], [106, 0, 44],[106, 269, 360], From a13c21ef2effd6219ccbf37d78b24a8b828ce9bb Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Fri, 22 Nov 2024 14:47:14 +0900 Subject: [PATCH 28/38] fix(aip_x2_gen2_launch): update left_lower point_filters config with MJ20-002 (#345) fix left_lower point_filters config with MJ20-002 data --- .../point_filters/left_lower/point_filters_full.param.yaml | 2 +- .../point_filters/left_lower/point_filters_one_third.param.yaml | 2 +- .../point_filters/left_lower/point_filters_two_third.param.yaml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml index b0d6c29d..94e2a9ed 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml @@ -31,7 +31,7 @@ [25, 0, 122],[25, 235, 360], [26, 0, 121],[26, 235, 360], [27, 0, 120],[27, 235, 360], - [28, 0, 119],[28, 240, 360], + [28, 0, 119],[28, 239, 360], [29, 0, 118],[29, 240, 360], [30, 0, 110],[30, 240, 360], [31, 0, 109],[31, 240, 265], diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml index db47c2ac..fc19ebef 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml @@ -31,7 +31,7 @@ [25, 0, 114],[25, 235, 360], [26, 0, 113],[26, 235, 360], [27, 0, 112],[27, 235, 360], - [28, 0, 119],[28, 240, 360], + [28, 0, 119],[28, 239, 360], [29, 0, 118],[29, 240, 360], [30, 0, 110],[30, 240, 360], [31, 0, 109],[31, 240, 265], diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml index 263deabf..62960b0f 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml @@ -31,7 +31,7 @@ [25, 0, 114],[25, 235, 360], [26, 0, 113],[26, 235, 360], [27, 0, 112],[27, 235, 360], - [28, 0, 119],[28, 240, 360], + [28, 0, 119],[28, 239, 360], [29, 0, 118],[29, 240, 360], [30, 0, 110],[30, 240, 360], [31, 0, 109],[31, 240, 265], From 3f5e7a02a83d975dc71d099456bc3e90e31de092 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Mon, 25 Nov 2024 15:07:39 +0900 Subject: [PATCH 29/38] fix(aip_x2_gen2_launch): fix front_upper cut_angle (#347) fix front_upper cut_angle --- aip_x2_gen2_launch/launch/lidar.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 512c84f7..0c3b3d2e 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -171,7 +171,7 @@ - + From 246b79312c6756668e3b392c74a572830f6cf141 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Mon, 25 Nov 2024 15:13:30 +0900 Subject: [PATCH 30/38] chore(aip_x2_gen2_launch): update point_filters with MJ20-005 rosbag (#346) * fix right_lower point_filters * fix left_upper point_filters * fix left_lower point_filters --- .../left_lower/point_filters_full.param.yaml | 13 ++++++----- .../point_filters_one_third.param.yaml | 11 ++++++---- .../point_filters_two_third.param.yaml | 11 ++++++---- .../left_upper/point_filters_full.param.yaml | 14 ++++++------ .../point_filters_one_third.param.yaml | 14 ++++++------ .../point_filters_two_third.param.yaml | 14 ++++++------ .../right_lower/point_filters_full.param.yaml | 20 ++++++++--------- .../point_filters_one_third.param.yaml | 22 +++++++++---------- .../point_filters_two_third.param.yaml | 22 +++++++++---------- 9 files changed, 75 insertions(+), 66 deletions(-) diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml index 94e2a9ed..943a4c5d 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml @@ -10,7 +10,7 @@ [4, 0, 155],[4, 203, 360], [5, 0, 152],[5, 206, 217],[5, 220, 360], [6, 0, 146],[6, 217, 360], - [7, 0, 143],[7, 235, 360], + [7, 0, 143],[7, 230, 360], [8, 0, 143],[8, 228.5, 360], [9, 0, 142],[9, 228.5, 360], [10, 0, 141],[10, 235, 360], @@ -34,9 +34,9 @@ [28, 0, 119],[28, 239, 360], [29, 0, 118],[29, 240, 360], [30, 0, 110],[30, 240, 360], - [31, 0, 109],[31, 240, 265], - [32, 0, 109],[32, 244, 265], - [33, 0, 108],[33, 244, 265], + [31, 0, 109],[31, 240, 267], + [32, 0, 109],[32, 244, 267], + [33, 0, 108],[33, 244, 267], [34, 0, 108],[34, 244, 265], [35, 0, 107],[35, 244, 265], [36, 0, 107],[36, 244, 265], @@ -54,6 +54,9 @@ [48, 0, 95], [49, 0, 94], [50, 0, 93], - [51, 0, 93] + [51, 0, 93], + [52, 0, 92], + [53, 0, 92], + [54, 0, 92] ] } diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml index fc19ebef..66e55a5f 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml @@ -10,7 +10,7 @@ [4, 0, 155],[4, 203, 360], [5, 0, 152],[5, 206, 217],[5, 220, 360], [6, 0, 146],[6, 217, 360], - [7, 0, 143],[7, 235, 360], + [7, 0, 143],[7, 230, 360], [8, 0, 139],[8, 228.5, 360], [9, 0, 135],[9, 228.5, 360], [10, 0, 133],[10, 235, 360], @@ -34,9 +34,9 @@ [28, 0, 119],[28, 239, 360], [29, 0, 118],[29, 240, 360], [30, 0, 110],[30, 240, 360], - [31, 0, 109],[31, 240, 265], - [32, 0, 109],[32, 244, 265], - [33, 0, 108],[33, 244, 265], + [31, 0, 109],[31, 240, 267], + [32, 0, 109],[32, 244, 267], + [33, 0, 108],[33, 244, 267], [34, 0, 108],[34, 244, 265], [35, 0, 107],[35, 244, 265], [36, 0, 107],[36, 244, 265], @@ -55,6 +55,9 @@ [49, 0, 94], [50, 0, 93], [51, 0, 93], + [52, 0, 92], + [53, 0, 92], + [54, 0, 92] [1, 0, 360], [2, 0, 360], [4, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml index 62960b0f..e21ce758 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml @@ -10,7 +10,7 @@ [4, 0, 155],[4, 203, 360], [5, 0, 152],[5, 206, 217],[5, 220, 360], [6, 0, 146],[6, 217, 360], - [7, 0, 143],[7, 235, 360], + [7, 0, 143],[7, 230, 360], [8, 0, 139],[8, 228.5, 360], [9, 0, 135],[9, 228.5, 360], [10, 0, 133],[10, 235, 360], @@ -34,9 +34,9 @@ [28, 0, 119],[28, 239, 360], [29, 0, 118],[29, 240, 360], [30, 0, 110],[30, 240, 360], - [31, 0, 109],[31, 240, 265], - [32, 0, 109],[32, 244, 265], - [33, 0, 108],[33, 244, 265], + [31, 0, 109],[31, 240, 267], + [32, 0, 109],[32, 244, 267], + [33, 0, 108],[33, 244, 267], [34, 0, 108],[34, 244, 265], [35, 0, 107],[35, 244, 265], [36, 0, 107],[36, 244, 265], @@ -55,6 +55,9 @@ [49, 0, 94], [50, 0, 93], [51, 0, 93], + [52, 0, 92], + [53, 0, 92], + [54, 0, 92], [0, 0, 360], [3, 0, 360], [6, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml index 366958ed..0303a1d4 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml @@ -61,12 +61,12 @@ [118, 303.81, 360], [119, 302.09, 360], [120, 298.42, 360], - [121, 299.63, 360], - [122, 298.42, 360], - [123, 298.42, 360], - [124, 293, 360], - [125, 90, 91],[125, 293, 360], - [126, 90, 91],[126, 292.12, 360], - [127, 90, 91],[127, 289.71, 360] + [121, 90, 91],[121, 299.63, 360], + [122, 90, 92],[122, 298.42, 360], + [123, 90, 92],[123, 298.42, 360], + [124, 90, 93],[124, 293, 360], + [125, 90, 93],[125, 293, 360], + [126, 90, 93],[126, 292.12, 360], + [127, 90, 93],[127, 289.71, 360] ] } diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml index d97f176e..d6117158 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml @@ -61,13 +61,13 @@ [118, 303.81, 360], [119, 302.09, 360], [120, 298.42, 360], - [121, 299.63, 360], - [122, 298.42, 360], - [123, 298.42, 360], - [124, 293, 360], - [125, 90, 91],[125, 293, 360], - [126, 90, 91],[126, 292.12, 360], - [127, 90, 91],[127, 289.71, 360], + [121, 90, 91],[121, 299.63, 360], + [122, 90, 92],[122, 298.42, 360], + [123, 90, 92],[123, 298.42, 360], + [124, 90, 93],[124, 293, 360], + [125, 90, 93],[125, 293, 360], + [126, 90, 93],[126, 292.12, 360], + [127, 90, 93],[127, 289.71, 360], [1, 0, 360], [2, 0, 360], [4, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml index 18753f9a..2022217a 100644 --- a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml @@ -61,13 +61,13 @@ [118, 303.81, 360], [119, 302.09, 360], [120, 298.42, 360], - [121, 299.63, 360], - [122, 298.42, 360], - [123, 298.42, 360], - [124, 293, 360], - [125, 90, 91],[125, 293, 360], - [126, 90, 91],[126, 292.12, 360], - [127, 90, 91],[127, 289.71, 360], + [121, 90, 91],[121, 299.63, 360], + [122, 90, 92],[122, 298.42, 360], + [123, 90, 92],[123, 298.42, 360], + [124, 90, 93],[124, 293, 360], + [125, 90, 93],[125, 293, 360], + [126, 90, 93],[126, 292.12, 360], + [127, 90, 93],[127, 289.71, 360], [0, 0, 360], [3, 0, 360], [6, 0, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml index e47031b9..ec2a65e9 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml @@ -20,18 +20,18 @@ [14, 0, 150],[14, 232, 360], [15, 0, 150],[15, 233, 360], [16, 0, 150],[16, 234, 360], - [17, 0, 145],[17, 235, 360], - [18, 0, 145],[18, 235, 360], - [19, 0, 135],[19, 236, 360], - [20, 102, 130],[20, 240, 360], + [17, 0, 147],[17, 235, 360], + [18, 0, 148],[18, 235, 360], + [19, 0, 146],[19, 236, 360], + [20, 97, 143],[20, 240, 360], [21, 102, 125],[21, 241, 360], [22, 105, 125],[22, 241, 360], - [23, 105, 124],[23, 244, 360], - [24, 105, 123],[24, 245, 360], - [25, 106, 122],[25, 246, 360], - [26, 107, 121],[26, 247, 360], - [27, 108, 120],[27, 248, 360], - [28, 109, 119],[28, 248, 360], + [23, 105, 125],[23, 244, 360], + [24, 105, 125],[24, 245, 360], + [25, 106, 124],[25, 246, 360], + [26, 107, 123],[26, 247, 360], + [27, 108, 122],[27, 248, 360], + [28, 109, 120],[28, 248, 360], [29, 110, 118],[29, 250, 360], [30, 111, 117],[30, 250, 360], [31, 112.5, 116],[31, 251, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml index 2a972111..6e755be6 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml @@ -20,18 +20,18 @@ [14, 0, 150],[14, 232, 360], [15, 0, 150],[15, 233, 360], [16, 0, 150],[16, 234, 360], - [17, 0, 145],[17, 235, 360], - [18, 0, 145],[18, 235, 360], - [19, 0, 135],[19, 236, 360], - [20, 102, 130],[20, 240, 360], - [21, 105, 125],[21, 241, 360], + [17, 0, 147],[17, 235, 360], + [18, 0, 148],[18, 235, 360], + [19, 0, 146],[19, 236, 360], + [20, 97, 143],[20, 240, 360], + [21, 102, 125],[21, 241, 360], [22, 105, 125],[22, 241, 360], - [23, 105, 124],[23, 244, 360], - [24, 105, 123],[24, 245, 360], - [25, 106, 122],[25, 246, 360], - [26, 107, 121],[26, 247, 360], - [27, 108, 120],[27, 248, 360], - [28, 109, 119],[28, 248, 360], + [23, 105, 125],[23, 244, 360], + [24, 105, 125],[24, 245, 360], + [25, 106, 124],[25, 246, 360], + [26, 107, 123],[26, 247, 360], + [27, 108, 122],[27, 248, 360], + [28, 109, 120],[28, 248, 360], [29, 110, 118],[29, 250, 360], [30, 111, 117],[30, 250, 360], [31, 112.5, 116],[31, 251, 360], diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml index c8b8fbbf..54889649 100644 --- a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml @@ -20,18 +20,18 @@ [14, 0, 150],[14, 232, 360], [15, 0, 150],[15, 233, 360], [16, 0, 150],[16, 234, 360], - [17, 0, 145],[17, 235, 360], - [18, 0, 145],[18, 235, 360], - [19, 0, 135],[19, 236, 360], - [20, 102, 130],[20, 240, 360], - [21, 105, 125],[21, 241, 360], + [17, 0, 147],[17, 235, 360], + [18, 0, 148],[18, 235, 360], + [19, 0, 146],[19, 236, 360], + [20, 97, 143],[20, 240, 360], + [21, 102, 125],[21, 241, 360], [22, 105, 125],[22, 241, 360], - [23, 105, 124],[23, 244, 360], - [24, 105, 123],[24, 245, 360], - [25, 106, 122],[25, 246, 360], - [26, 107, 121],[26, 247, 360], - [27, 108, 120],[27, 248, 360], - [28, 109, 119],[28, 248, 360], + [23, 105, 125],[23, 244, 360], + [24, 105, 125],[24, 245, 360], + [25, 106, 124],[25, 246, 360], + [26, 107, 123],[26, 247, 360], + [27, 108, 122],[27, 248, 360], + [28, 109, 120],[28, 248, 360], [29, 110, 118],[29, 250, 360], [30, 111, 117],[30, 250, 360], [31, 112.5, 116],[31, 251, 360], From 358588c7576ba65b178993fbf9b7ec1335e1b244 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Thu, 28 Nov 2024 16:21:55 +0900 Subject: [PATCH 31/38] Chore/lidar enable blockage diag (#350) * fix vertical_bins * enable blockage_diag * fix params for blockage_diag --- .../launch/hesai_OT128.launch.xml | 2 +- .../launch/hesai_QT128.launch.xml | 2 +- aip_x2_gen2_launch/launch/lidar.launch.xml | 42 +++++++++---------- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index e9cd6805..9d82aff6 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -22,7 +22,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index 80c73b0b..07bdfbe7 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -23,7 +23,7 @@ - + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 0c3b3d2e..33981799 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -11,7 +11,7 @@ - + @@ -48,8 +48,8 @@ - - + + @@ -82,8 +82,8 @@ - - + + @@ -116,9 +116,9 @@ - - - + + + @@ -150,9 +150,9 @@ - - - + + + @@ -185,8 +185,8 @@ - - + + @@ -219,8 +219,8 @@ - - + + @@ -253,9 +253,9 @@ - - - + + + @@ -287,9 +287,9 @@ - - - + + + From f711c1385a8aa90003ab948b8dc121ca4913becd Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 28 Nov 2024 23:58:29 +0900 Subject: [PATCH 32/38] fix: cut angle for left_upper and front_upper (#349) * fix: cut angle for left_upper and front_upper Signed-off-by: badai-nguyen * chore: adjust cut angle Signed-off-by: badai-nguyen * fix: make sure cut_angle equal max angle Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- aip_x2_gen2_launch/launch/lidar.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 33981799..18618abe 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -68,9 +68,9 @@ - + - + From 6a5916b8339b3ebbefc4aa4171d5e354a2b9faae Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Thu, 12 Dec 2024 18:53:36 +0900 Subject: [PATCH 33/38] fix(aip_x2_gen2_launch): point filters default (#356) fix default folder name --- .../{dafault => default}/point_filters_full.param.yaml | 0 .../{dafault => default}/point_filters_left_upper.param.yaml | 0 .../{dafault => default}/point_filters_one_half.param.yaml | 0 .../{dafault => default}/point_filters_one_third.param.yaml | 0 .../{dafault => default}/point_filters_right_upper.param.yaml | 0 .../{dafault => default}/point_filters_two_third.param.yaml | 0 6 files changed, 0 insertions(+), 0 deletions(-) rename aip_x2_gen2_launch/config/point_filters/{dafault => default}/point_filters_full.param.yaml (100%) rename aip_x2_gen2_launch/config/point_filters/{dafault => default}/point_filters_left_upper.param.yaml (100%) rename aip_x2_gen2_launch/config/point_filters/{dafault => default}/point_filters_one_half.param.yaml (100%) rename aip_x2_gen2_launch/config/point_filters/{dafault => default}/point_filters_one_third.param.yaml (100%) rename aip_x2_gen2_launch/config/point_filters/{dafault => default}/point_filters_right_upper.param.yaml (100%) rename aip_x2_gen2_launch/config/point_filters/{dafault => default}/point_filters_two_third.param.yaml (100%) diff --git a/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_full.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters/dafault/point_filters_full.param.yaml rename to aip_x2_gen2_launch/config/point_filters/default/point_filters_full.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_left_upper.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_left_upper.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters/dafault/point_filters_left_upper.param.yaml rename to aip_x2_gen2_launch/config/point_filters/default/point_filters_left_upper.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_one_half.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_one_half.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters/dafault/point_filters_one_half.param.yaml rename to aip_x2_gen2_launch/config/point_filters/default/point_filters_one_half.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_one_third.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters/dafault/point_filters_one_third.param.yaml rename to aip_x2_gen2_launch/config/point_filters/default/point_filters_one_third.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_right_upper.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_right_upper.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters/dafault/point_filters_right_upper.param.yaml rename to aip_x2_gen2_launch/config/point_filters/default/point_filters_right_upper.param.yaml diff --git a/aip_x2_gen2_launch/config/point_filters/dafault/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_two_third.param.yaml similarity index 100% rename from aip_x2_gen2_launch/config/point_filters/dafault/point_filters_two_third.param.yaml rename to aip_x2_gen2_launch/config/point_filters/default/point_filters_two_third.param.yaml From 1fa1482afc04296a6b4d7293055f665cb61c9c37 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Thu, 12 Dec 2024 18:59:17 +0900 Subject: [PATCH 34/38] fix(aip_x2_gen2_launch): front_upper filter (#358) fix front_upper filter --- .../point_filters/front_upper/point_filters_full.param.yaml | 2 +- .../front_upper/point_filters_one_third.param.yaml | 2 +- .../front_upper/point_filters_two_third.param.yaml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml index 630b2ae4..9ba6571d 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml @@ -36,7 +36,7 @@ [45, 0, 93],[45, 267, 360], [50, 0, 98], [51, 0, 98], - [52, 0, 98],[52, 195, 360], + [52, 0, 98], [53, 0, 98], [54, 0, 98], [57, 0, 93], diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml index ddb36703..70496d1e 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml @@ -36,7 +36,7 @@ [45, 0, 93],[45, 267, 360], [50, 0, 98], [51, 0, 98], - [52, 0, 98],[52, 195, 360], + [52, 0, 98], [53, 0, 98], [54, 0, 98], [57, 0, 93], diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml index 02a1e4ee..3ae4770d 100644 --- a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml @@ -36,7 +36,7 @@ [45, 0, 93],[45, 267, 360], [50, 0, 98], [51, 0, 98], - [52, 0, 98],[52, 195, 360], + [52, 0, 98], [53, 0, 98], [54, 0, 98], [57, 0, 93], From af5f318ea61e4fde09b8492cc7ced4c473a082e8 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Fri, 13 Dec 2024 01:34:05 +0900 Subject: [PATCH 35/38] feat: temporary radar support for x2 gen2 v4.0.1 (#357) * disable motion cancel in converter node because sensor has already done it * remove left/right radar for temporary usage Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- aip_x2_gen2_launch/config/simple_object_merger.param.yaml | 2 +- .../config/radar_tracks_msgs_converter.param.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x2_gen2_launch/config/simple_object_merger.param.yaml b/aip_x2_gen2_launch/config/simple_object_merger.param.yaml index a817e0b7..b85c9ce6 100644 --- a/aip_x2_gen2_launch/config/simple_object_merger.param.yaml +++ b/aip_x2_gen2_launch/config/simple_object_merger.param.yaml @@ -3,4 +3,4 @@ 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"] + input_topics: ["/sensing/radar/front_center/detected_objects", "/sensing/radar/rear_center/detected_objects"] diff --git a/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml b/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml index 7b344aaa..933cdd0b 100644 --- a/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml +++ b/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml @@ -2,6 +2,6 @@ ros__parameters: update_rate_hz: 20.0 new_frame_id: "base_link" - use_twist_compensation: true + use_twist_compensation: false use_twist_yaw_compensation: false static_object_speed_threshold: 1.0 From 77694d10d433bd1492a9d95255b2eee7f99dafdd Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Wed, 18 Dec 2024 15:55:18 +0900 Subject: [PATCH 36/38] fix(aip_x2_gen2_launch): lidar distacne range (#359) fix lidar distance range --- aip_x2_gen2_launch/launch/hesai_OT128.launch.xml | 7 +++++-- aip_x2_gen2_launch/launch/hesai_QT128.launch.xml | 8 +++++--- aip_x2_gen2_launch/launch/lidar.launch.xml | 15 --------------- 3 files changed, 10 insertions(+), 20 deletions(-) diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index 9d82aff6..4adc9311 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -19,8 +19,9 @@ - - + + + @@ -57,6 +58,8 @@ + + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index 07bdfbe7..443294ba 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -20,8 +20,9 @@ - - + + + @@ -56,7 +57,8 @@ - + + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 18618abe..6065d9fd 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -46,7 +46,6 @@ - @@ -79,8 +78,6 @@ - - @@ -113,8 +110,6 @@ - - @@ -147,8 +142,6 @@ - - @@ -182,8 +175,6 @@ - - @@ -216,8 +207,6 @@ - - @@ -250,8 +239,6 @@ - - @@ -284,8 +271,6 @@ - - From 6295f20ada25a9695eab4271e6625c673893adb9 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Fri, 27 Dec 2024 19:10:52 +0900 Subject: [PATCH 37/38] chore(aip_x2_gen2_launch): fix blockage_angle (#363) fix blockage_angle --- .../config/blockage_diagnostics_param_file.yaml | 2 +- aip_x2_gen2_launch/launch/lidar.launch.xml | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml index e43b2039..ea0285a5 100644 --- a/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml +++ b/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - blockage_ratio_threshold: 0.1 + blockage_ratio_threshold: 0.2 blockage_count_threshold: 50 blockage_buffering_frames: 2 blockage_buffering_interval: 1 diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml index 6065d9fd..bddb9a2a 100644 --- a/aip_x2_gen2_launch/launch/lidar.launch.xml +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -110,7 +110,7 @@ - + @@ -142,7 +142,7 @@ - + @@ -239,7 +239,7 @@ - + @@ -271,7 +271,7 @@ - + From 51e69410aa4bb6b12d5496bda5fd1b87600f3103 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 10 Jan 2025 19:02:05 +0900 Subject: [PATCH 38/38] chore: fix pre-commit errors (#368) * chore: fix pre-commit errors Signed-off-by: Tomohito Ando * ci(pre-commit): autofix --------- Signed-off-by: Tomohito Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../launch/nebula_node_container.launch.py | 57 ------------------- 1 file changed, 57 deletions(-) diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py index d228e11c..ad58a838 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -56,13 +56,6 @@ def get_vehicle_info(context): 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: @@ -154,51 +147,6 @@ def str2vector(string): 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"], - ) - - right_mirror_crop_component = 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"], - ) - - left_mirror_crop_component = 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")}], - ) - undistort_component = ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::DistortionCorrectorComponent", @@ -275,8 +223,6 @@ def str2vector(string): glog_component, nebula_component, self_crop_component, - # right_mirror_crop_component, - # left_mirror_crop_component, undistort_component, ], ) @@ -343,9 +289,6 @@ def add_launch_arg(name: str, default_value=None, description=None): 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("use_multithread", "False", "use multithread") add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication")