From 4b3a37d5556ff1be27f011b7bac393463ba22d94 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 7 Nov 2024 19:26:05 +0900 Subject: [PATCH 1/6] feat: awsim load comcatenate parameter Signed-off-by: vividf --- .../concatenate_and_time_sync_node.param.yaml | 22 ++++++++++++ .../launch/pointcloud_preprocessor.launch.py | 35 +++++++++++-------- 2 files changed, 42 insertions(+), 15 deletions(-) create mode 100644 awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml diff --git a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 0000000..35e5df4 --- /dev/null +++ b/awsim_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: false + 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", # 0.05 + "/sensing/lidar/top/pointcloud_before_sync", # 0.05 + "/sensing/lidar/left/pointcloud_before_sync", # 0.05 + ] + output_frame: base_link + lidar_timestamp_offsets: [0.0, 0.0, 0.0] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01] \ No newline at end of file diff --git a/awsim_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/awsim_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index 6bfec90..bbabebf 100644 --- a/awsim_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/awsim_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,16 @@ 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", @@ -37,19 +46,7 @@ def launch_setup(context, *args, **kwargs): ), ("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, - "timeout_sec": 0.01, - } - ], + parameters=[concatenate_and_time_sync_node_param], extra_arguments=[ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], @@ -73,10 +70,18 @@ def add_launch_arg(name: str, default_value=None): DeclareLaunchArgument(name, default_value=default_value) ) - add_launch_arg("base_frame", "base_link") + awsim_sensor_kit_launch_share_dir = get_package_share_directory("awsim_sensor_kit_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( + awsim_sensor_kit_launch_share_dir, + "config", + "concatenate_and_time_sync_node.param.yaml", + ), + ) set_container_executable = SetLaunchConfiguration( "container_executable", From ba5878702183e2995fae1e32ed2126a17c637cd8 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 7 Nov 2024 19:27:18 +0900 Subject: [PATCH 2/6] chore: add space Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 35e5df4..3605ef5 100644 --- a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml @@ -19,4 +19,4 @@ ] output_frame: base_link lidar_timestamp_offsets: [0.0, 0.0, 0.0] - lidar_timestamp_noise_window: [0.01, 0.01, 0.01] \ No newline at end of file + lidar_timestamp_noise_window: [0.01, 0.01, 0.01] From 01f20e84303601aedd7eb35fc8f028fcfe428752 Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 11 Nov 2024 14:41:17 +0900 Subject: [PATCH 3/6] chore: fix timeout sec Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 3605ef5..39dc86d 100644 --- a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml @@ -5,7 +5,7 @@ rosbag_replay: false rosbag_length: 20.0 maximum_queue_size: 5 - timeout_sec: 0.2 + timeout_sec: 0.01 is_motion_compensated: false publish_synchronized_pointcloud: true keep_input_frame_in_synchronized_pointcloud: true @@ -13,9 +13,9 @@ synchronized_pointcloud_postfix: pointcloud input_twist_topic_type: twist input_topics: [ - "/sensing/lidar/right/pointcloud_before_sync", # 0.05 + "/sensing/lidar/right/pointcloud_before_sync", # Fill this after using the right point cloud "/sensing/lidar/top/pointcloud_before_sync", # 0.05 - "/sensing/lidar/left/pointcloud_before_sync", # 0.05 + "/sensing/lidar/left/pointcloud_before_sync", # Fill this after using the left point cloud ] output_frame: base_link lidar_timestamp_offsets: [0.0, 0.0, 0.0] From 3c617a7321cab354a99447c680af4eedc88ccd95 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 4 Dec 2024 12:21:28 +0900 Subject: [PATCH 4/6] 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/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 39dc86d..bb527cd 100644 --- a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_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.01 is_motion_compensated: false From 81ed40e3c55c0b52741438875d05b70e912c683e Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 9 Dec 2024 12:53:54 +0900 Subject: [PATCH 5/6] 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/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index bb527cd..818dac2 100644 --- a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_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 9b151ef5e914dec828a79eda643a675af4b1907e Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 12 Dec 2024 14:44:27 +0900 Subject: [PATCH 6/6] chore: add mathcing strategy Signed-off-by: vividf --- .../config/concatenate_and_time_sync_node.param.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 818dac2..fcfc743 100644 --- a/awsim_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_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", # Fill this after using the left point cloud ] output_frame: base_link - lidar_timestamp_offsets: [0.0, 0.0, 0.0] - lidar_timestamp_noise_window: [0.01, 0.01, 0.01] + matching_strategy: + type: advanced + lidar_timestamp_offsets: [0.0, 0.0, 0.0] + lidar_timestamp_noise_window: [0.01, 0.01, 0.01] \ No newline at end of file