From 474738df8a1357270653c0dd92aea2af6d16916e Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 5 Feb 2025 14:11:53 +0900 Subject: [PATCH 1/6] feat(aip_x2_gen2_launch): update launch to run new concatenate node Signed-off-by: Tomohito Ando --- .../concatenate_and_time_sync_node.param.yaml | 28 ++++++++++++ .../launch/pointcloud_preprocessor.launch.py | 43 +++++++++++-------- 2 files changed, 52 insertions(+), 19 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..fac6393b --- /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.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.075 + "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.074 + "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.090 + "/sensing/lidar/right_lower/pointcloud_before_sync", # 0.00 + ] + output_frame: base_link + 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_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", From 53750f5961f25ba22a17d19d2421b0285cdc63f1 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 5 Feb 2025 14:12:35 +0900 Subject: [PATCH 2/6] change output topic name Signed-off-by: Tomohito Ando --- 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 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=[ { From 619bac3044331bdffa7e10c3b2c4a9fd0909e506 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 5 Feb 2025 14:30:28 +0900 Subject: [PATCH 3/6] update param file for x2 gen2 Signed-off-by: Tomohito Ando --- .../concatenate_and_time_sync_node.param.yaml | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) 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 index fac6393b..71c53a5f 100644 --- 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 @@ -5,24 +5,24 @@ rosbag_length: 10.0 maximum_queue_size: 5 timeout_sec: 0.2 - is_motion_compensated: true - publish_synchronized_pointcloud: true + is_motion_compensated: false + publish_synchronized_pointcloud: false 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.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.075 - "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.074 - "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.090 - "/sensing/lidar/right_lower/pointcloud_before_sync", # 0.00 + "/sensing/lidar/left_lower/pointcloud_before_sync", + "/sensing/lidar/left_upper/pointcloud_before_sync", + "/sensing/lidar/front_lower/pointcloud_before_sync", + "/sensing/lidar/front_upper/pointcloud_before_sync", + "/sensing/lidar/right_upper/pointcloud_before_sync", + "/sensing/lidar/right_lower/pointcloud_before_sync", + "/sensing/lidar/rear_lower/pointcloud_before_sync", + "/sensing/lidar/rear_upper/pointcloud_before_sync" ] output_frame: base_link 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_offsets: [0.0, 0.0, 0.025, 0.028, 0.026, 0.05, 0.075, 0.076] lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] From b39640e3598e65fe9df0ccb4ef4603b399f05250 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 5 Feb 2025 14:40:32 +0900 Subject: [PATCH 4/6] change `is_motion_compensated` and `publish_synchronized_pointcloud` to true Signed-off-by: Tomohito Ando --- .../config/concatenate_and_time_sync_node.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index 71c53a5f..4553eff7 100644 --- 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 @@ -5,8 +5,8 @@ rosbag_length: 10.0 maximum_queue_size: 5 timeout_sec: 0.2 - is_motion_compensated: false - publish_synchronized_pointcloud: false + 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 From fcb43fd6cfb27db7976d80eef09c94540ab7c5c6 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 5 Feb 2025 16:29:44 +0900 Subject: [PATCH 5/6] update param Signed-off-by: Tomohito Ando --- .../concatenate_and_time_sync_node.param.yaml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) 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 index 4553eff7..c2700a3f 100644 --- 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 @@ -12,17 +12,17 @@ synchronized_pointcloud_postfix: pointcloud input_twist_topic_type: twist input_topics: [ - "/sensing/lidar/left_lower/pointcloud_before_sync", - "/sensing/lidar/left_upper/pointcloud_before_sync", - "/sensing/lidar/front_lower/pointcloud_before_sync", - "/sensing/lidar/front_upper/pointcloud_before_sync", - "/sensing/lidar/right_upper/pointcloud_before_sync", - "/sensing/lidar/right_lower/pointcloud_before_sync", - "/sensing/lidar/rear_lower/pointcloud_before_sync", - "/sensing/lidar/rear_upper/pointcloud_before_sync" + "/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.0, 0.025, 0.028, 0.026, 0.05, 0.075, 0.076] + lidar_timestamp_offsets: [0.0, 0.01, 0.01, 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] From 7dcbd1ec5d9806ba425dba97540c9b3e819ba8b2 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 5 Feb 2025 16:34:42 +0900 Subject: [PATCH 6/6] Update aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml --- .../config/concatenate_and_time_sync_node.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index c2700a3f..c0a9a892 100644 --- 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 @@ -24,5 +24,5 @@ output_frame: base_link matching_strategy: type: advanced - lidar_timestamp_offsets: [0.0, 0.01, 0.01, 0.0, 0.025, 0.025, 0.026, 0.050] + 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]