From c5dbc573042f8b5e3fffd09ef948c8fb28149904 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Tue, 28 Jan 2025 19:30:48 +0900 Subject: [PATCH 01/10] chore(aip_x2_gen2_launch): remove distance_range (#378) remove distance_range --- 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 | 3 +-- 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml index 4adc9311..8bbaddca 100644 --- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -21,7 +21,6 @@ - @@ -60,7 +59,6 @@ - diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml index 443294ba..67b9f0b8 100644 --- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -22,7 +22,6 @@ - @@ -59,7 +58,6 @@ - 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 6f17007b..d48d62cd 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -191,7 +191,6 @@ def str2vector(string): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range"))) blockage_diag_component = ComposableNode( package="autoware_pointcloud_preprocessor", plugin="autoware::pointcloud_preprocessor::BlockageDiagComponent", @@ -206,7 +205,7 @@ def str2vector(string): "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], + "max_distance_range": LaunchConfiguration("max_range"), "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), } ] From c5d3aa430a484690c8e0b1334a55d10dbae54acf Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 30 Jan 2025 20:30:00 +0900 Subject: [PATCH 02/10] chore(common): remove "launch_hw_monitor" param in hesai_*.launch.xml (#381) remove "launch_hw_monitor" param in hesai_*.launch.xml Signed-off-by: Autumn60 --- common_sensor_launch/launch/hesai_OT128.launch.xml | 3 --- common_sensor_launch/launch/hesai_XT32.launch.xml | 3 --- 2 files changed, 6 deletions(-) diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml index dcb260f4..1d0df1db 100644 --- a/common_sensor_launch/launch/hesai_OT128.launch.xml +++ b/common_sensor_launch/launch/hesai_OT128.launch.xml @@ -2,9 +2,6 @@ - - diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml index e7e93a1b..f507497b 100644 --- a/common_sensor_launch/launch/hesai_XT32.launch.xml +++ b/common_sensor_launch/launch/hesai_XT32.launch.xml @@ -2,9 +2,6 @@ - - From e457bc3ff37cc74389d32029b4c8db16d4717932 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 30 Jan 2025 20:47:49 +0900 Subject: [PATCH 03/10] fix(common): remove missing params in hesai_*.launch.xml (#382) remove missing params in hesai_*.launch.xml Signed-off-by: Autumn60 --- common_sensor_launch/launch/hesai_OT128.launch.xml | 1 - common_sensor_launch/launch/hesai_XT32.launch.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml index 1d0df1db..7df4f126 100644 --- a/common_sensor_launch/launch/hesai_OT128.launch.xml +++ b/common_sensor_launch/launch/hesai_OT128.launch.xml @@ -18,7 +18,6 @@ - diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml index f507497b..3155cd1b 100644 --- a/common_sensor_launch/launch/hesai_XT32.launch.xml +++ b/common_sensor_launch/launch/hesai_XT32.launch.xml @@ -18,7 +18,6 @@ - From ecaa657053d1a8a1fe5b422d0020cbec74dcdccc Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 30 Jan 2025 21:11:11 +0900 Subject: [PATCH 04/10] chore(common): format hesai launch (#383) * explicit declaration for max_range param in hesai_XT32.launch.xml Signed-off-by: Autumn60 * sort params in hesai_*.launch.xml Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- common_sensor_launch/launch/hesai_OT128.launch.xml | 4 ++-- common_sensor_launch/launch/hesai_XT32.launch.xml | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml index 7df4f126..fee67fe1 100644 --- a/common_sensor_launch/launch/hesai_OT128.launch.xml +++ b/common_sensor_launch/launch/hesai_OT128.launch.xml @@ -19,8 +19,8 @@ - + @@ -29,9 +29,9 @@ + - diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml index 3155cd1b..afed4851 100644 --- a/common_sensor_launch/launch/hesai_XT32.launch.xml +++ b/common_sensor_launch/launch/hesai_XT32.launch.xml @@ -12,6 +12,7 @@ + @@ -19,8 +20,8 @@ - + @@ -30,6 +31,7 @@ + @@ -37,6 +39,5 @@ - From c1f84fd6fe6217a2b6931ecc131da5c86bb8a87d Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Fri, 31 Jan 2025 11:48:47 +0900 Subject: [PATCH 05/10] chore(aip_x1): remove "use_radius_search" param in lidar.launch.xml (#380) remove "use_radius_search" param in lidar.launch.xml Signed-off-by: Autumn60 --- aip_x1_launch/launch/lidar.launch.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index f1796100..4c31e1d0 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -3,7 +3,6 @@ - From fdbcf80ee33a33670345a203766ef9c4dc366208 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Fri, 31 Jan 2025 11:48:57 +0900 Subject: [PATCH 06/10] chore: sort dummy diags to match aggregator config (#379) sort dummy diags Signed-off-by: Autumn60 --- .../sensor_kit.param.yaml | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index 75712024..be704546 100644 --- a/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -12,6 +12,19 @@ /**: ros__parameters: required_diags: + # sensing + sensing_topic_status: default + + # velodyne + velodyne_connection: default + velodyne_temperature: default + velodyne_rpm: default + + # pandar + pandar_connection: default + pandar_temperature: default + pandar_ptp: default + # gnss gnss_connection: default gnss_data: default @@ -21,17 +34,5 @@ gnss_jamming: default fix topic status: default - # pandar - pandar_connection: default - pandar_temperature: default - pandar_ptp: default - - # velodyne - velodyne_connection: default - velodyne_temperature: default - velodyne_rpm: default - - sensing_topic_status: default - # imu gyro_bias_estimator: default From 474ad262f194e862d634d5c30b9ad26756aa5243 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 4 Feb 2025 07:52:25 +0900 Subject: [PATCH 07/10] fix(aip_x2_gen2_launch): add `autoware_` prefix to glog component (#384) 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 d48d62cd..6497123f 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -76,7 +76,7 @@ def str2vector(string): glog_component = ComposableNode( package="autoware_glog_component", - plugin="GlogComponent", + plugin="autoware::glog_component::GlogComponent", name="glog_component", ) From d7b7b1733b07ef9183a4555b835aaeb44b328bf5 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 4 Feb 2025 07:55:49 +0900 Subject: [PATCH 08/10] chore(aip_x2_gen2_launch): use parameter file for ring_outlier_filter (#385) Signed-off-by: Tomohito Ando --- .../launch/nebula_node_container.launch.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) 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 6497123f..835af5e7 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -26,6 +26,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare import yaml @@ -161,6 +162,18 @@ def str2vector(string): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + ring_outlier_filter_node_param = ParameterFile( + param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").perform(context), + allow_substs=True, + ) + + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here + if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true": + ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} + else: + # keep the output frame as the input frame + ring_outlier_output_frame = {"output_frame": ""} + ring_outlier_filter_component = ComposableNode( package="autoware_pointcloud_preprocessor", plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent", @@ -169,6 +182,7 @@ def str2vector(string): ("input", "rectified/pointcloud_ex"), ("output", "pointcloud"), ], + parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -298,6 +312,10 @@ def add_launch_arg(name: str, default_value=None, description=None): "blockage_diagnostics_param_file", [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics.param.yaml"], ) + add_launch_arg( + "ring_outlier_filter_node_param_file", + [FindPackageShare("common_sensor_launch"), "/config/ring_outlier_filter_node.param.yaml"], + ) add_launch_arg( "distortion_corrector_node_param_file", [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], @@ -312,6 +330,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("point_filters_param_file") add_launch_arg("calibration_file", "") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") set_container_executable = SetLaunchConfiguration( "container_executable", From 52cec8737c2bef5faacbf788f7595d0a11eb1e3e Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 4 Feb 2025 11:34:12 +0900 Subject: [PATCH 09/10] feat: throttled the radar inputs (#371) feat: throttled the radar inputs following indications from the documentation and switched to sensor time Signed-off-by: Kenzo Lobos-Tsunekawa --- aip_x2_gen2_launch/config/ARS548.param.yaml | 6 +-- aip_x2_gen2_launch/launch/radar.launch.xml | 44 ++++++++++++--------- 2 files changed, 29 insertions(+), 21 deletions(-) diff --git a/aip_x2_gen2_launch/config/ARS548.param.yaml b/aip_x2_gen2_launch/config/ARS548.param.yaml index 1fe5e7e2..03943890 100644 --- a/aip_x2_gen2_launch/config/ARS548.param.yaml +++ b/aip_x2_gen2_launch/config/ARS548.param.yaml @@ -10,8 +10,8 @@ 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 + use_sensor_time: true + configuration_vehicle_length: 7.2369 # wheel_base + front_overhang + rear_overhang + configuration_vehicle_width: 2.2916 # wheel_tread + left_overhang + right_overhang configuration_vehicle_height: 3.08 configuration_vehicle_wheelbase: 4.76012 diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml index d793d8de..599076a9 100644 --- a/aip_x2_gen2_launch/launch/radar.launch.xml +++ b/aip_x2_gen2_launch/launch/radar.launch.xml @@ -7,6 +7,14 @@ + + + + + + + + @@ -14,9 +22,9 @@ - - - + + + @@ -45,9 +53,9 @@ - - - + + + @@ -75,9 +83,9 @@ - - - + + + @@ -105,9 +113,9 @@ - - - + + + @@ -135,9 +143,9 @@ - - - + + + @@ -165,9 +173,9 @@ - - - + + + From 22e633c292072f79da0fe8971ce0bd031e0f9ae7 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 5 Feb 2025 16:44:04 +0900 Subject: [PATCH 10/10] feat(aip_x2_gen2_launch): update launch to run new concatenate node (#386) * feat(aip_x2_gen2_launch): update launch to run new concatenate node Signed-off-by: Tomohito Ando * change output topic name Signed-off-by: Tomohito Ando * update param file for x2 gen2 Signed-off-by: Tomohito Ando * change `is_motion_compensated` and `publish_synchronized_pointcloud` to true Signed-off-by: Tomohito Ando * update param Signed-off-by: Tomohito Ando * Update aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml --------- Signed-off-by: Tomohito Ando Co-authored-by: Yi-Hsiang Fang (Vivid) <146902905+vividf@users.noreply.github.com> --- .../concatenate_and_time_sync_node.param.yaml | 28 ++++++++++++ .../launch/nebula_node_container.launch.py | 4 +- .../launch/pointcloud_preprocessor.launch.py | 43 +++++++++++-------- 3 files changed, 54 insertions(+), 21 deletions(-) create mode 100644 aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml diff --git a/aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 00000000..c0a9a892 --- /dev/null +++ b/aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + debug_mode: false + has_static_tf_only: false + rosbag_length: 10.0 + maximum_queue_size: 5 + timeout_sec: 0.2 + is_motion_compensated: true + publish_synchronized_pointcloud: true + keep_input_frame_in_synchronized_pointcloud: true + publish_previous_but_late_pointcloud: false + synchronized_pointcloud_postfix: pointcloud + input_twist_topic_type: twist + input_topics: [ + "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.047 + "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.048 + "/sensing/lidar/left_upper/pointcloud_before_sync", # 0.048 + "/sensing/lidar/left_lower/pointcloud_before_sync", # 0.047 + "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.072 + "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.072 + "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.073 + "/sensing/lidar/right_lower/pointcloud_before_sync" # 0.097 + ] + output_frame: base_link + matching_strategy: + type: advanced + lidar_timestamp_offsets: [0.0, 0.001, 0.001, 0.0, 0.025, 0.025, 0.026, 0.050] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 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 835af5e7..e8a4a55a 100644 --- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -180,7 +180,7 @@ def str2vector(string): name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -192,7 +192,7 @@ def str2vector(string): name="dual_return_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], parameters=[ { diff --git a/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py index 0775e3a2..dbac5d8e 100644 --- a/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py @@ -12,7 +12,9 @@ # 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 @@ -22,9 +24,18 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterFile def launch_setup(context, *args, **kwargs): + # concatenate node parameters + concatenate_and_time_sync_node_param = ParameterFile( + param_file=LaunchConfiguration("concatenate_and_time_sync_node_param_path").perform( + context + ), + allow_substs=True, + ) + # set concat filter as a component concat_component = ComposableNode( package="autoware_pointcloud_preprocessor", @@ -34,30 +45,15 @@ def launch_setup(context, *args, **kwargs): ("~/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", - } - ], + parameters=[concatenate_and_time_sync_node_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # load concat or passthrough filter concat_loader = LoadComposableNodes( composable_node_descriptions=[concat_component], target_container=LaunchConfiguration("pointcloud_container_name"), + condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) return [concat_loader] @@ -69,10 +65,19 @@ def generate_launch_description(): 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") + aip_x2_gen2_launch_share_dir = get_package_share_directory("aip_x2_gen2_launch") + add_launch_arg("use_multithread", "True") add_launch_arg("use_intra_process", "True") add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg( + "concatenate_and_time_sync_node_param_path", + os.path.join( + aip_x2_gen2_launch_share_dir, + "config", + "concatenate_and_time_sync_node.param.yaml", + ), + ) set_container_executable = SetLaunchConfiguration( "container_executable",