From 2e379ea0beed552f23ab0247c10e10c247ccf120 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 7 Nov 2024 19:19:37 +0900 Subject: [PATCH 1/5] feat: awsim lab load concatenate parameter Signed-off-by: vividf --- .../concatenate_and_time_sync_node.param.yaml | 22 +++++++++++ .../launch/pointcloud_preprocessor.launch.py | 37 +++++++++++-------- 2 files changed, 43 insertions(+), 16 deletions(-) create mode 100644 awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml diff --git a/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 0000000..3605ef5 --- /dev/null +++ b/awsim_labs_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] diff --git a/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index 6e9a2a7..1b9749e 100644 --- a/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/awsim_labs_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,10 +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", @@ -35,19 +43,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"), - "timeout_sec": 0.01, - "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") + awsim_labs_sensor_kit_launch_share_dir = get_package_share_directory("awsim_labs_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_labs_sensor_kit_launch_share_dir, + "config", + "concatenate_and_time_sync_node.param.yaml", + ), + ) set_container_executable = SetLaunchConfiguration( "container_executable", From 1d1f49708b7799b1dd4daee799ca94566d203bd4 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 4 Dec 2024 12:23:10 +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/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 3605ef5..cba0cbf 100644 --- a/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_labs_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: false From dadb1bc3ff138ecc6c9a7abebf0c45a8c9c0ac1e Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 9 Dec 2024 12:52:35 +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/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index cba0cbf..399df54 100644 --- a/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_labs_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 ef6bd689ce9c17d45b47c103fbb0cf391b2a0437 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 12 Dec 2024 14:45:58 +0900 Subject: [PATCH 4/5] 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_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 399df54..4437dc8 100644 --- a/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_labs_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", # 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] + 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 From 6ddc9759cd1ea066ac11e665ae823313d647ae2e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 16 Dec 2024 12:35:35 +0000 Subject: [PATCH 5/5] style(pre-commit): autofix --- .../config/concatenate_and_time_sync_node.param.yaml | 2 +- .../launch/pointcloud_preprocessor.launch.py | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml index 4437dc8..e1c3d1f 100644 --- a/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/awsim_labs_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml @@ -20,4 +20,4 @@ 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 + lidar_timestamp_noise_window: [0.01, 0.01, 0.01] diff --git a/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index 1b9749e..1758fe3 100644 --- a/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -26,6 +26,7 @@ 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( @@ -64,7 +65,9 @@ def generate_launch_description(): def add_launch_arg(name: str, default_value=None): launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - awsim_labs_sensor_kit_launch_share_dir = get_package_share_directory("awsim_labs_sensor_kit_launch") + awsim_labs_sensor_kit_launch_share_dir = get_package_share_directory( + "awsim_labs_sensor_kit_launch" + ) add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False")