From b0c157b0a422ad7ed68305a8a2a3ac30ed4fc79e Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 30 Oct 2024 20:18:35 +0900 Subject: [PATCH 1/9] feat: initialize parameters for concatenate Signed-off-by: vividf --- .../concatenate_and_time_sync_node.param.yaml | 21 +++++++++ .../launch/pointcloud_preprocessor.launch.py | 39 +++++++++-------- .../concatenate_and_time_sync_node.param.yaml | 27 ++++++++++++ .../launch/pointcloud_preprocessor.launch.py | 43 ++++++++++--------- .../concatenate_and_time_sync_node.param.yaml | 23 ++++++++++ .../launch/pointcloud_preprocessor.launch.py | 42 +++++++++--------- .../concatenate_and_time_sync_node.param.yaml | 23 ++++++++++ .../launch/pointcloud_preprocessor.launch.py | 43 ++++++++++--------- 8 files changed, 182 insertions(+), 79 deletions(-) create mode 100644 aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml create mode 100644 aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml create mode 100644 aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml create mode 100644 aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml diff --git a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 00000000..e2c74c1b --- /dev/null +++ b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + debug_mode: false + has_static_tf_only: false + rosbag_replay: false + rosbag_length: 20.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/top/pointcloud_before_sync", # unknown timestamp + "/sensing/lidar/front_center/pointcloud_before_sync", # unknown timestamp + ] + output_frame: base_link + lidar_timestamp_offsets: [0.0, 0.02] # unknown offset + lidar_timestamp_noise_window: [0.01, 0.01] diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index 23af3493..ff400622 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -12,6 +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 @@ -21,9 +24,17 @@ 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", @@ -33,22 +44,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("output", "concatenated/pointcloud"), ], - parameters=[ - { - "input_topics": [ - "/sensing/lidar/top/pointcloud_before_sync", - "/sensing/lidar/front_center/pointcloud_before_sync", - ], - "output_frame": LaunchConfiguration("base_frame"), - "input_offset": [ - 0.055, - 0.025, - ], - "timeout_sec": 0.095, - "input_twist_topic_type": "twist", - "publish_synchronized_pointcloud": True, - } - ], + parameters=[concatenate_and_time_sync_node_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -68,10 +64,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_x1_launch_share_dir = get_package_share_directory("aip_x1_launch") + add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg( + "concatenate_and_time_sync_node_param_path", + os.path.join( + aip_x1_launch_share_dir, + "config", + "concatenate_and_time_sync_node.param.yaml", + ), + ) set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 00000000..54c04501 --- /dev/null +++ b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -0,0 +1,27 @@ +/**: + ros__parameters: + debug_mode: false + has_static_tf_only: false + rosbag_replay: false + rosbag_length: 20.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/left_upper/pointcloud_before_sync", # 0.05 + "/sensing/lidar/left_lower/pointcloud_before_sync", # 0.05 + "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.025 + "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.074 + "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.091 + "/sensing/lidar/right_lower/pointcloud_before_sync", # 0.00 + "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.055 + "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.065 + ] + output_frame: base_link + lidar_timestamp_offsets: [0.0, 0.0, 0.075, 0.024, 0.041, 0.05, 0.005, 0.015] + 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_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py index 827d68ab..3f9705cb 100644 --- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_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,25 +45,7 @@ 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_before_sync", - "/sensing/lidar/front_lower/pointcloud_before_sync", - "/sensing/lidar/left_upper/pointcloud_before_sync", - "/sensing/lidar/left_lower/pointcloud_before_sync", - "/sensing/lidar/right_upper/pointcloud_before_sync", - "/sensing/lidar/right_lower/pointcloud_before_sync", - "/sensing/lidar/rear_upper/pointcloud_before_sync", - "/sensing/lidar/rear_lower/pointcloud_before_sync", - ], - "input_offset": [0.025, 0.025, 0.01, 0.0, 0.05, 0.05, 0.05, 0.05], - "timeout_sec": 0.075, - "output_frame": LaunchConfiguration("base_frame"), - "input_twist_topic_type": "twist", - "publish_synchronized_pointcloud": True, - } - ], + parameters=[concatenate_and_time_sync_node_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -72,11 +65,21 @@ 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_launch_share_dir = get_package_share_directory("aip_x2_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_launch_share_dir, + "config", + "concatenate_and_time_sync_node.param.yaml", + ), + ) + set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 00000000..1bd1d0a0 --- /dev/null +++ b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + debug_mode: false + has_static_tf_only: false + rosbag_replay: false + rosbag_length: 20.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/top/pointcloud", # unknown timestamp + "/sensing/lidar/left/pointcloud", # unknown timestamp + "/sensing/lidar/right/pointcloud", # unknown timestamp + "/sensing/lidar/rear/pointcloud", # unknown timestamp + ] + output_frame: base_link + lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02] # unknown offset + lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01] diff --git a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py index 224e90a5..04e5eca8 100644 --- a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_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,20 +45,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("output", "concatenated/pointcloud"), ], - parameters=[ - { - "input_topics": LaunchConfiguration("input_topics"), - "output_frame": LaunchConfiguration("base_frame"), - "input_offset": LaunchConfiguration( - "input_offset" - ), # each sensor will wait 60, 70, 70, 70ms - "timeout_sec": LaunchConfiguration("timeout_sec"), # set shorter than 100ms - "input_twist_topic_type": LaunchConfiguration("input_twist_topic_type"), - "publish_synchronized_pointcloud": LaunchConfiguration( - "publish_synchronized_pointcloud" - ), - } - ], + parameters=[concatenate_and_time_sync_node_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -67,19 +65,21 @@ 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_xx1_gen2_launch_share_dir = get_package_share_directory("aip_xx1_gen2_launch") + add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") add_launch_arg("pointcloud_container_name", "pointcloud_container") add_launch_arg("individual_container_name", "concatenate_container") + add_launch_arg( - "input_topics", - "[/sensing/lidar/top/pointcloud, /sensing/lidar/left/pointcloud, /sensing/lidar/right/pointcloud, /sensing/lidar/rear/pointcloud]", + "concatenate_and_time_sync_node_param_path", + os.path.join( + aip_xx1_gen2_launch_share_dir, + "config", + "concatenate_and_time_sync_node.param.yaml", + ), ) - add_launch_arg("input_offset", "[0.035, 0.025, 0.025, 0.025]") - add_launch_arg("timeout_sec", "0.095") - add_launch_arg("input_twist_topic_type", "twist") - add_launch_arg("publish_synchronized_pointcloud", "False") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 00000000..86ef53b0 --- /dev/null +++ b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + debug_mode: false + has_static_tf_only: false + rosbag_replay: false + rosbag_length: 20.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/top/pointcloud_before_sync", # 0.08 + "/sensing/lidar/left/pointcloud_before_sync", # 0.00 + "/sensing/lidar/right/pointcloud_before_sync", # 0.00 + "/sensing/lidar/rear/pointcloud_before_sync", # 0.00 + ] + output_frame: base_link + lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01] diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index 521fe463..d1fa63a1 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_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,26 +45,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("output", "concatenated/pointcloud"), ], - parameters=[ - { - "input_topics": [ - "/sensing/lidar/top/pointcloud_before_sync", - "/sensing/lidar/left/pointcloud_before_sync", - "/sensing/lidar/right/pointcloud_before_sync", - "/sensing/lidar/rear/pointcloud_before_sync", - ], - "output_frame": LaunchConfiguration("base_frame"), - "input_offset": [ - 0.035, - 0.025, - 0.025, - 0.025, - ], # each sensor will wait 60, 70, 70, 70ms - "timeout_sec": 0.095, # set shorter than 100ms - "input_twist_topic_type": "twist", - "publish_synchronized_pointcloud": True, - } - ], + parameters=[concatenate_and_time_sync_node_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -73,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_xx1_launch_share_dir = get_package_share_directory("aip_xx1_launch") + add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg( + "concatenate_and_time_sync_node_param_path", + os.path.join( + aip_xx1_launch_share_dir, + "config", + "concatenate_and_time_sync_node.param.yaml", + ), + ) set_container_executable = SetLaunchConfiguration( "container_executable", From 6e59490519dc0fdb3cd44e073454534d7560e310 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 31 Oct 2024 14:36:48 +0900 Subject: [PATCH 2/9] chore: add xx1 gen2 parameters Signed-off-by: vividf --- .../concatenate_and_time_sync_node.param.yaml | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml index 1bd1d0a0..bd1dc498 100644 --- a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -6,18 +6,19 @@ rosbag_length: 20.0 maximum_queue_size: 5 timeout_sec: 0.2 - is_motion_compensated: true + is_motion_compensated: false # no need to compensate for motion as lidar scan at the same time. 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/top/pointcloud", # unknown timestamp - "/sensing/lidar/left/pointcloud", # unknown timestamp - "/sensing/lidar/right/pointcloud", # unknown timestamp - "/sensing/lidar/rear/pointcloud", # unknown timestamp + "/sensing/lidar/top/pointcloud_before_sync", # 0.99 + "/sensing/lidar/front_left/pointcloud_before_sync", # 0.99 + "/sensing/lidar/front_right/pointcloud_before_sync", # 0.99 + "/sensing/lidar/side_left/pointcloud_before_sync", # 0.99 + "/sensing/lidar/side_right/pointcloud_before_sync", # 0.99 ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02] # unknown offset - lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01] + lidar_timestamp_offsets: [0.0, 0.0, 0.0, 0.0, 0.0] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01] From 00d8bf15942ce82f638cd97f4d830d9a4226824c Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 1 Nov 2024 17:41:37 +0900 Subject: [PATCH 3/9] chore: fix aip x1 parameter Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml index e2c74c1b..cb5e873c 100644 --- a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -13,9 +13,9 @@ synchronized_pointcloud_postfix: pointcloud input_twist_topic_type: twist input_topics: [ - "/sensing/lidar/top/pointcloud_before_sync", # unknown timestamp - "/sensing/lidar/front_center/pointcloud_before_sync", # unknown timestamp + "/sensing/lidar/front_center/pointcloud_before_sync", # 0.025 + "/sensing/lidar/top/pointcloud_before_sync", # 0.055 ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.02] # unknown offset + lidar_timestamp_offsets: [0.0, 0.03] lidar_timestamp_noise_window: [0.01, 0.01] From 74a076601ac5e09f4a67514b2e9803ba48b647ff Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 7 Nov 2024 01:00:18 +0900 Subject: [PATCH 4/9] chore: fix x2 parameter Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml index 54c04501..f726354b 100644 --- a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -13,15 +13,15 @@ synchronized_pointcloud_postfix: pointcloud input_twist_topic_type: twist input_topics: [ + "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.044 + "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.049 "/sensing/lidar/left_upper/pointcloud_before_sync", # 0.05 "/sensing/lidar/left_lower/pointcloud_before_sync", # 0.05 - "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.025 + "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.075 "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.074 - "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.091 + "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.090 "/sensing/lidar/right_lower/pointcloud_before_sync", # 0.00 - "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.055 - "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.065 ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.0, 0.075, 0.024, 0.041, 0.05, 0.005, 0.015] + lidar_timestamp_offsets: [0.0, 0.005, 0.006, 0.006, 0.031, 0.03, 0.046, 0.056] lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] From f780e9cbb1a0032abf637f876c7c0cb210499d26 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 4 Dec 2024 11:25:29 +0900 Subject: [PATCH 5/9] chore: increase noise window value for xx1 Signed-off-by: vividf --- aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml index 86ef53b0..185e8be0 100644 --- a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -20,4 +20,4 @@ ] output_frame: base_link lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02] - lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01] + lidar_timestamp_noise_window: [0.02, 0.02, 0.02, 0.02] From eae683d7b084a4d81182e9261a170233d3b8bc27 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 4 Dec 2024 12:11:02 +0900 Subject: [PATCH 6/9] chore: update parameters Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 3 +-- .../config/concatenate_and_time_sync_node.param.yaml | 3 +-- .../config/concatenate_and_time_sync_node.param.yaml | 3 +-- .../config/concatenate_and_time_sync_node.param.yaml | 11 +++++------ 4 files changed, 8 insertions(+), 12 deletions(-) diff --git a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml index cb5e873c..a31bc4ea 100644 --- a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -2,8 +2,7 @@ ros__parameters: debug_mode: false has_static_tf_only: false - rosbag_replay: false - rosbag_length: 20.0 + rosbag_length: 10.0 maximum_queue_size: 5 timeout_sec: 0.2 is_motion_compensated: true diff --git a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml index f726354b..5e70a7c6 100644 --- a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -2,8 +2,7 @@ ros__parameters: debug_mode: false has_static_tf_only: false - rosbag_replay: false - rosbag_length: 20.0 + rosbag_length: 10.0 maximum_queue_size: 5 timeout_sec: 0.2 is_motion_compensated: true diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml index bd1dc498..ce8c3c18 100644 --- a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -2,8 +2,7 @@ ros__parameters: debug_mode: false has_static_tf_only: false - rosbag_replay: false - rosbag_length: 20.0 + rosbag_length: 10.0 maximum_queue_size: 5 timeout_sec: 0.2 is_motion_compensated: false # no need to compensate for motion as lidar scan at the same time. diff --git a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml index 185e8be0..e27e4072 100644 --- a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -2,8 +2,7 @@ ros__parameters: debug_mode: false has_static_tf_only: false - rosbag_replay: false - rosbag_length: 20.0 + rosbag_length: 10.0 maximum_queue_size: 5 timeout_sec: 0.2 is_motion_compensated: true @@ -13,10 +12,10 @@ synchronized_pointcloud_postfix: pointcloud input_twist_topic_type: twist input_topics: [ - "/sensing/lidar/top/pointcloud_before_sync", # 0.08 - "/sensing/lidar/left/pointcloud_before_sync", # 0.00 - "/sensing/lidar/right/pointcloud_before_sync", # 0.00 - "/sensing/lidar/rear/pointcloud_before_sync", # 0.00 + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/left/pointcloud_before_sync", + "/sensing/lidar/right/pointcloud_before_sync", + "/sensing/lidar/rear/pointcloud_before_sync", ] output_frame: base_link lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02] From ce4513f32553703c31782a8cc1328b705114755b Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 9 Dec 2024 12:51:01 +0900 Subject: [PATCH 7/9] chore: add use_naive_approach Signed-off-by: vividf --- aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml | 2 ++ aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml | 2 ++ .../config/concatenate_and_time_sync_node.param.yaml | 2 ++ aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml | 2 ++ 4 files changed, 8 insertions(+) diff --git a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml index a31bc4ea..374a65fa 100644 --- a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true + use_naive_approach: false debug_mode: false has_static_tf_only: false rosbag_length: 10.0 diff --git a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml index 5e70a7c6..fe37a3d9 100644 --- a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true + use_naive_approach: false debug_mode: false has_static_tf_only: false rosbag_length: 10.0 diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml index ce8c3c18..e889deef 100644 --- a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true + use_naive_approach: false debug_mode: false has_static_tf_only: false rosbag_length: 10.0 diff --git a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml index e27e4072..54745186 100644 --- a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true + use_naive_approach: false debug_mode: false has_static_tf_only: false rosbag_length: 10.0 From edf7946232bc218b52619f98808aa3c4b91ae2de Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 12 Dec 2024 14:41:39 +0900 Subject: [PATCH 8/9] chore: add matching strategy parameter Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 8 ++++---- .../config/concatenate_and_time_sync_node.param.yaml | 8 ++++---- .../config/concatenate_and_time_sync_node.param.yaml | 8 ++++---- .../config/concatenate_and_time_sync_node.param.yaml | 8 ++++---- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml index 374a65fa..fcc00d60 100644 --- a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,7 +1,5 @@ /**: ros__parameters: - # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true - use_naive_approach: false debug_mode: false has_static_tf_only: false rosbag_length: 10.0 @@ -18,5 +16,7 @@ "/sensing/lidar/top/pointcloud_before_sync", # 0.055 ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.03] - lidar_timestamp_noise_window: [0.01, 0.01] + matching_strategy: + type: advanced + lidar_timestamp_offsets: [0.0, 0.03] + lidar_timestamp_noise_window: [0.01, 0.01] diff --git a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml index fe37a3d9..fac6393b 100644 --- a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,7 +1,5 @@ /**: ros__parameters: - # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true - use_naive_approach: false debug_mode: false has_static_tf_only: false rosbag_length: 10.0 @@ -24,5 +22,7 @@ "/sensing/lidar/right_lower/pointcloud_before_sync", # 0.00 ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.005, 0.006, 0.006, 0.031, 0.03, 0.046, 0.056] - lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + matching_strategy: + type: advanced + lidar_timestamp_offsets: [0.0, 0.005, 0.006, 0.006, 0.031, 0.03, 0.046, 0.056] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml index e889deef..9e5ffa7e 100644 --- a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,7 +1,5 @@ /**: ros__parameters: - # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true - use_naive_approach: false debug_mode: false has_static_tf_only: false rosbag_length: 10.0 @@ -21,5 +19,7 @@ "/sensing/lidar/side_right/pointcloud_before_sync", # 0.99 ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.0, 0.0, 0.0, 0.0] - lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01] + matching_strategy: + type: advanced + lidar_timestamp_offsets: [0.0, 0.0, 0.0, 0.0, 0.0] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01] \ No newline at end of file diff --git a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml index 54745186..453c5408 100644 --- a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,7 +1,5 @@ /**: ros__parameters: - # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true - use_naive_approach: false debug_mode: false has_static_tf_only: false rosbag_length: 10.0 @@ -20,5 +18,7 @@ "/sensing/lidar/rear/pointcloud_before_sync", ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02] - lidar_timestamp_noise_window: [0.02, 0.02, 0.02, 0.02] + matching_strategy: + type: advanced + lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02] + lidar_timestamp_noise_window: [0.02, 0.02, 0.02, 0.02] From 50e8a696e3671cf31fad861375c6a8b38f7d3950 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 12 Dec 2024 05:44:02 +0000 Subject: [PATCH 9/9] ci(pre-commit): autofix --- .../config/concatenate_and_time_sync_node.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml index 9e5ffa7e..f6a0030f 100644 --- a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -22,4 +22,4 @@ matching_strategy: type: advanced lidar_timestamp_offsets: [0.0, 0.0, 0.0, 0.0, 0.0] - lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01] \ No newline at end of file + lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01]