From e677d39849b36c95619352c20ac5275a0147a3a2 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 13 May 2024 21:18:56 +0900 Subject: [PATCH] feat(update): update lidar pointcloud interface and concatenate pointcloud filter (#215) * feat: update common nebula container Signed-off-by: yoshiri * feat: fix xx1 pointcloud preprocessor launch Signed-off-by: yoshiri * feat: update x1 launcher to use new interface Signed-off-by: yoshiri * feat: update x2 launcher Signed-off-by: yoshiri * Update aip_x2_launch/launch/pandar_node_container.launch.py Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> * fix: fix x2 topics disconnection Signed-off-by: yoshiri --------- Signed-off-by: yoshiri Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> --- .../launch/new_livox_horizon.launch.py | 4 ++-- .../launch/pointcloud_preprocessor.launch.py | 9 +++++---- .../launch/velodyne_node_container.launch.py | 11 ++++++++++- .../launch/pandar_node_container.launch.py | 13 +++++++++++-- .../launch/pointcloud_preprocessor.launch.py | 17 +++++++++-------- .../launch/pointcloud_preprocessor.launch.py | 9 +++++---- .../launch/nebula_node_container.launch.py | 11 ++++++++++- 7 files changed, 52 insertions(+), 22 deletions(-) diff --git a/aip_x1_launch/launch/new_livox_horizon.launch.py b/aip_x1_launch/launch/new_livox_horizon.launch.py index de3f8b9a..922d23ba 100644 --- a/aip_x1_launch/launch/new_livox_horizon.launch.py +++ b/aip_x1_launch/launch/new_livox_horizon.launch.py @@ -52,12 +52,12 @@ def get_crop_box_min_range_component(context, livox_frame_id): name="crop_box_filter_min_range", remappings=[ ("input", "livox/tag_filtered/lidar" if use_tag_filter else "livox/lidar"), - ("output", "min_range_cropped/pointcloud"), + ("output", "min_range_cropped/pointcloud_before_sync"), ], parameters=[ { "input_frame": livox_frame_id, - "output_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("frame_id"), "min_x": 0.0, "max_x": LaunchConfiguration("min_range"), "min_y": -2.0, diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index 4a3ad9e1..3c186976 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -37,14 +37,15 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/front_left/min_range_cropped/pointcloud", - "/sensing/lidar/front_right/min_range_cropped/pointcloud", - "/sensing/lidar/front_center/min_range_cropped/pointcloud", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/front_left/min_range_cropped/pointcloud_before_sync", + "/sensing/lidar/front_right/min_range_cropped/pointcloud_before_sync", + "/sensing/lidar/front_center/min_range_cropped/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py index 56b35116..2ce274b1 100644 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_launch/launch/velodyne_node_container.launch.py @@ -121,6 +121,13 @@ def create_parameter_dict(*args): ) ) + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here + if LaunchConfiguration("output_as_sensor_frame").perform(context): + ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + else: + ring_outlier_filter_parameters = { + "output_frame": "" + } # keep the output frame as the input frame nodes.append( ComposableNode( package="pointcloud_preprocessor", @@ -128,8 +135,9 @@ def create_parameter_dict(*args): name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], + parameters=[ring_outlier_filter_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -218,6 +226,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg("use_multithread", "False", "use multithread") add_launch_arg("use_intra_process", "False", "use ROS2 component container communication") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index 8d8b13cf..e32719be 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -206,14 +206,22 @@ def create_parameter_dict(*args): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here + if LaunchConfiguration("output_as_sensor_frame").perform(context): + ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + else: + ring_outlier_filter_parameters = { + "output_frame": "" + } # keep the output frame as the input frame ring_outlier_filter_component = ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::RingOutlierFilterComponent", name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], + parameters=[ring_outlier_filter_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -223,7 +231,7 @@ def create_parameter_dict(*args): name="dual_return_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], parameters=[ { @@ -345,6 +353,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("min_azimuth_deg", "135.0") add_launch_arg("max_azimuth_deg", "225.0") add_launch_arg("enable_blockage_diag", "true") + add_launch_arg("output_as_sensor_frame", "True") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py index 4ef586ff..bb205119 100644 --- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py @@ -38,19 +38,20 @@ def launch_setup(context, *args, **kwargs): 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", + "/sensing/lidar/front_upper/pointcloud_before_sync", + "/sensing/lidar/front_lower/pointcloud_before_sync", + "/sensing/lidar/left_upper/pointcloud_before_sync", + "/sensing/lidar/left_lower/pointcloud_before_sync", + "/sensing/lidar/right_upper/pointcloud_before_sync", + "/sensing/lidar/right_lower/pointcloud_before_sync", + "/sensing/lidar/rear_upper/pointcloud_before_sync", + "/sensing/lidar/rear_lower/pointcloud_before_sync", ], "input_offset": [0.025, 0.025, 0.01, 0.0, 0.05, 0.05, 0.05, 0.05], "timeout_sec": 0.075, "output_frame": LaunchConfiguration("base_frame"), "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index a2a0fd45..3628d60a 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -38,10 +38,10 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/left/pointcloud", - "/sensing/lidar/right/pointcloud", - "/sensing/lidar/rear/pointcloud", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/left/pointcloud_before_sync", + "/sensing/lidar/right/pointcloud_before_sync", + "/sensing/lidar/rear/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), "input_offset": [ @@ -52,6 +52,7 @@ def launch_setup(context, *args, **kwargs): ], # each sensor will wait 60, 70, 70, 70ms "timeout_sec": 0.095, # set shorter than 100ms "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index a79681b8..4099768c 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -180,6 +180,13 @@ def create_parameter_dict(*args): ) ) + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here + if LaunchConfiguration("output_as_sensor_frame").perform(context): + ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + else: + ring_outlier_filter_parameters = { + "output_frame": "" + } # keep the output frame as the input frame nodes.append( ComposableNode( package="pointcloud_preprocessor", @@ -187,8 +194,9 @@ def create_parameter_dict(*args): name="ring_outlier_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], + parameters=[ring_outlier_filter_parameters], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -289,6 +297,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("use_pointcloud_container", "false") add_launch_arg("container_name", "nebula_node_container") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") set_container_executable = SetLaunchConfiguration( "container_executable",