diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index ea35c49d..ed7b3bf5 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -38,22 +38,23 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/side_left/pointcloud", - "/sensing/lidar/side_right/pointcloud", - "/sensing/lidar/front_left/pointcloud", - "/sensing/lidar/front_right/pointcloud", + "/sensing/lidar/top/pointcloud_before_sync", + # "/sensing/lidar/side_left/pointcloud_before_sync", + # "/sensing/lidar/side_right/pointcloud_before_sync", + # "/sensing/lidar/front_left/pointcloud_before_sync", + # "/sensing/lidar/front_right/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), "input_offset": [ 0.035, - 0.025, - 0.025, - 0.025, - 0.025, + # 0.025, + # 0.025, + # 0.025, + # 0.025, ], # 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 abb72290..9047cf18 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -115,6 +115,8 @@ def create_parameter_dict(*args): "cloud_min_angle", "cloud_max_angle", "dual_return_distance_threshold", + "setup_sensor", + "retry_hw", ), }, ], @@ -126,6 +128,36 @@ def create_parameter_dict(*args): ) ) + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + **create_parameter_dict( + "return_mode", + "frame_id", + "scan_phase", + "sensor_ip", + "host_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "rotation_speed", + "cloud_min_angle", + "cloud_max_angle", + "diag_span", + "dual_return_distance_threshold", + "delay_monitor_ms", + ), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -188,6 +220,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 +234,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")}], ) ) @@ -236,6 +276,7 @@ def create_parameter_dict(*args): "setup_sensor", "ptp_profile", "ptp_transport_type", + "retry_hw", ), } ], @@ -263,6 +304,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("config_file", "", description="sensor configuration file") add_launch_arg("launch_driver", "True", "do launch driver") add_launch_arg("setup_sensor", "True", "configure sensor") + add_launch_arg("retry_hw", "false", "retry hw") add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") add_launch_arg("host_ip", "255.255.255.255", "host ip address") add_launch_arg("scan_phase", "0.0") @@ -287,6 +329,9 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("container_name", "nebula_node_container") add_launch_arg("ptp_profile", "1588v2") add_launch_arg("ptp_transport_type", "L2") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") + add_launch_arg("diag_span", "1000", "") + add_launch_arg("delay_monitor_ms", "2000", "") set_container_executable = SetLaunchConfiguration( "container_executable",