From f1b729468a390ee0bd47987b2e2a36c1f1da8922 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 18 Jul 2023 01:08:25 +0900 Subject: [PATCH 1/5] apply concatenate node update --- .../launch/pointcloud_preprocessor.launch.py | 95 +++++++++++++++---- 1 file changed, 74 insertions(+), 21 deletions(-) diff --git a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index 779a1f12..e1e63901 100644 --- a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -27,26 +27,79 @@ def launch_setup(context, *args, **kwargs): # set concat filter as a component - concat_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_data", - remappings=[ - ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("output", "concatenated/pointcloud"), - ], - parameters=[ - { - "input_topics": [ - "/sensing/lidar/top/outlier_filtered/pointcloud", - "/sensing/lidar/left/outlier_filtered/pointcloud", - "/sensing/lidar/right/outlier_filtered/pointcloud", - ], - "output_frame": LaunchConfiguration("base_frame"), - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) + separate_concatenate_node_and_timesync_node_str = DeclareLaunchArgument( + "separate_concatenate_node_and_timesync_node", + default_value="false", + description="Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", + ) + separate_concatenate_node_and_timesync_node = separate_concatenate_node_and_timesync_node_str.lower() == "true" + + # switch between sync_and_concatenate_filter and synchronizer_filter + if not separate_concatenate_node_and_timesync_node: + sync_and_concat_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="sync_and_concatenate_filter", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "concatenated/pointcloud"), + ], + parameters=[ + { + "input_topics": [ + "/sensing/lidar/top/outlier_filtered/pointcloud", + "/sensing/lidar/left/outlier_filtered/pointcloud", + "/sensing/lidar/right/outlier_filtered/pointcloud", + ], + "output_frame": LaunchConfiguration("base_frame"), + "approximate_sync": True, + "publish_synchronized_pointcloud": True, # set true if you want to publish synchronized pointcloud + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + concat_components = [sync_and_concat_component] + else: + time_sync_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent", + name="synchronizer_filter", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ], + parameters=[ + { + "input_topics": [ + "/sensing/lidar/top/outlier_filtered/pointcloud", + "/sensing/lidar/left/outlier_filtered/pointcloud", + "/sensing/lidar/right/outlier_filtered/pointcloud", + ], + "output_frame": LaunchConfiguration("base_frame"), + "approximate_sync": True, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + concat_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenationComponent", + name="concatenate_filter", + remappings=[("output", "points_raw/concatenated")], + parameters=[ + { + "input_topics": [ + "/sensing/lidar/top/outlier_filtered/pointcloud_synchronized", + "/sensing/lidar/left/outlier_filtered/pointcloud_synchronized", + "/sensing/lidar/right/outlier_filtered/pointcloud_synchronized", + ], + "output_frame": LaunchConfiguration("base_frame"), + "approximate_sync": True, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + concat_components = [time_sync_component, concat_component] # set container to run all required components in the same process container = ComposableNodeContainer( @@ -67,7 +120,7 @@ def launch_setup(context, *args, **kwargs): # load concat or passthrough filter concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component], + composable_node_descriptions=concat_components, target_container=target_container, condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) From 9331697be4703a5e2bcbc7d0579a7a3bcad273bd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 17 Jul 2023 16:14:10 +0000 Subject: [PATCH 2/5] style(pre-commit): autofix --- .../launch/pointcloud_preprocessor.launch.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index e1e63901..64f09f5e 100644 --- a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -28,11 +28,13 @@ def launch_setup(context, *args, **kwargs): # set concat filter as a component separate_concatenate_node_and_timesync_node_str = DeclareLaunchArgument( - "separate_concatenate_node_and_timesync_node", - default_value="false", - description="Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", - ) - separate_concatenate_node_and_timesync_node = separate_concatenate_node_and_timesync_node_str.lower() == "true" + "separate_concatenate_node_and_timesync_node", + default_value="false", + description="Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", + ) + separate_concatenate_node_and_timesync_node = ( + separate_concatenate_node_and_timesync_node_str.lower() == "true" + ) # switch between sync_and_concatenate_filter and synchronizer_filter if not separate_concatenate_node_and_timesync_node: @@ -53,7 +55,7 @@ def launch_setup(context, *args, **kwargs): ], "output_frame": LaunchConfiguration("base_frame"), "approximate_sync": True, - "publish_synchronized_pointcloud": True, # set true if you want to publish synchronized pointcloud + "publish_synchronized_pointcloud": True, # set true if you want to publish synchronized pointcloud } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], From 0e7fff840fc432350b1d4de3f9a8a3a9da3e4ea9 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 25 Jul 2023 12:09:06 +0900 Subject: [PATCH 3/5] fix spelling and sign off Signed-off-by: yoshiri --- .../launch/pointcloud_preprocessor.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index 64f09f5e..913f1383 100644 --- a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -26,11 +26,11 @@ def launch_setup(context, *args, **kwargs): - # set concat filter as a component + # set concatenate filter as a component separate_concatenate_node_and_timesync_node_str = DeclareLaunchArgument( "separate_concatenate_node_and_timesync_node", default_value="false", - description="Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", + description="Set True to separate concatenate node and time synchronization node. which will cause to larger memory usage.", ) separate_concatenate_node_and_timesync_node = ( separate_concatenate_node_and_timesync_node_str.lower() == "true" From b7108788922ab11e60650de125c0f576ddb91178 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 25 Jul 2023 16:00:59 +0900 Subject: [PATCH 4/5] fix config load error Signed-off-by: yoshiri --- .../launch/pointcloud_preprocessor.launch.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index 913f1383..07593493 100644 --- a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -27,14 +27,9 @@ def launch_setup(context, *args, **kwargs): # set concatenate filter as a component - separate_concatenate_node_and_timesync_node_str = DeclareLaunchArgument( - "separate_concatenate_node_and_timesync_node", - default_value="false", - description="Set True to separate concatenate node and time synchronization node. which will cause to larger memory usage.", - ) - separate_concatenate_node_and_timesync_node = ( - separate_concatenate_node_and_timesync_node_str.lower() == "true" - ) + + separate_concatenate_node_and_timesync_node_str = LaunchConfiguration("separate_concatenate_node_and_timesync_node").perform(context) + separate_concatenate_node_and_timesync_node = separate_concatenate_node_and_timesync_node_str.lower() == "true" # switch between sync_and_concatenate_filter and synchronizer_filter if not separate_concatenate_node_and_timesync_node: @@ -141,6 +136,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "False") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_preprocessor_container") + add_launch_arg("separate_concatenate_node_and_timesync_node", "False") set_container_executable = SetLaunchConfiguration( "container_executable", From b79faef832a7ee1e1319ecc7ee8d33d195c3e293 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 25 Jul 2023 07:01:14 +0000 Subject: [PATCH 5/5] style(pre-commit): autofix --- .../launch/pointcloud_preprocessor.launch.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index 07593493..af7e50f3 100644 --- a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -28,8 +28,12 @@ def launch_setup(context, *args, **kwargs): # set concatenate filter as a component - separate_concatenate_node_and_timesync_node_str = LaunchConfiguration("separate_concatenate_node_and_timesync_node").perform(context) - separate_concatenate_node_and_timesync_node = separate_concatenate_node_and_timesync_node_str.lower() == "true" + separate_concatenate_node_and_timesync_node_str = LaunchConfiguration( + "separate_concatenate_node_and_timesync_node" + ).perform(context) + separate_concatenate_node_and_timesync_node = ( + separate_concatenate_node_and_timesync_node_str.lower() == "true" + ) # switch between sync_and_concatenate_filter and synchronizer_filter if not separate_concatenate_node_and_timesync_node: