From db5489e9f5b8bacd71b0a048e6d52e388e2cf2b7 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 29 Oct 2024 15:34:26 +0900 Subject: [PATCH 1/5] feat: concatenate node load from parameter file Signed-off-by: vividf --- .../concatenate_and_time_sync_node.param.yaml | 22 ++++++++++++ .../launch/pointcloud_preprocessor.launch.py | 34 ++++++++++++------- 2 files changed, 44 insertions(+), 12 deletions(-) create mode 100644 sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml diff --git a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 00000000..c5bd139e --- /dev/null +++ b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml @@ -0,0 +1,22 @@ +/**: + 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/right/pointcloud_before_sync", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/left/pointcloud_before_sync", + ] + output_frame: base_link + lidar_timestamp_offsets: [0.0, 0.015, 0.016] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01] diff --git a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index c29d74e3..8dbcfc99 100644 --- a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/sample_sensor_kit_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,18 +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", - ], - "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")}], ) @@ -65,10 +65,20 @@ def generate_launch_description(): def add_launch_arg(name: str, default_value=None): launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + sample_sensor_kit_launch_share_dir = get_package_share_directory("sample_sensor_kit_launch") + add_launch_arg("base_frame", "base_link") 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( + sample_sensor_kit_launch_share_dir, + "config", + "concatenate_and_time_sync_node.param.yaml", + ), + ) set_container_executable = SetLaunchConfiguration( "container_executable", From aaafd80c032b666ecda51de0dca50a144804f173 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 4 Dec 2024 12:13:12 +0900 Subject: [PATCH 2/5] chore: update params Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index c5bd139e..909845d5 100644 --- a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/sample_sensor_kit_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 From 8989ca370bf3aa376b3f162bd12fb5cf828bde75 Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 9 Dec 2024 12:46:24 +0900 Subject: [PATCH 3/5] chore: add use_naive_approach Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 909845d5..12093b8b 100644 --- a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/sample_sensor_kit_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 a7e6fcce737afbca16429c573d06cbe993aaa11e Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 9 Dec 2024 12:49:10 +0900 Subject: [PATCH 4/5] chore: remove space Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 12093b8b..fc5a0f69 100644 --- a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - # ignore the lidar_timestamp_offsets and lidar_timestamp_noise_window if 'use_naive_approach' is true + # 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 From 22a33d1178a6513056fafa6f1403aec190dc931f Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 12 Dec 2024 12:03:42 +0900 Subject: [PATCH 5/5] feat: add matching strategy params Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index fc5a0f69..cec253e1 100644 --- a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/sample_sensor_kit_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 @@ -19,5 +17,7 @@ "/sensing/lidar/left/pointcloud_before_sync", ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.015, 0.016] - lidar_timestamp_noise_window: [0.01, 0.01, 0.01] + matching_strategy: + type: advanced + lidar_timestamp_offsets: [0.0, 0.015, 0.016] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01]