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 ff012c8a..8d799261 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -36,14 +36,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 cfb39cc4..defdf75d 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -202,14 +202,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")}], ) @@ -219,7 +227,7 @@ def create_parameter_dict(*args): name="dual_return_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], parameters=[ { @@ -336,6 +344,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 6d435dee..3c0a09d8 100644 --- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py @@ -37,19 +37,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 10542a54..726171f9 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -37,10 +37,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": [ @@ -51,6 +51,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 c14c1390..891b6989 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -188,6 +188,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", @@ -195,8 +202,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")}], ) ) @@ -283,6 +291,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 ROS 2 component container communication") 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",