#![cfg(feature = "ouster-test")]
use anyhow::Result;
use lidar_utils::ouster::{
config::Config, frame_converter::FrameConverter, packet::Packet as OusterPacket,
pcd_converter::PointCloudConverter,
};
use pcap::Capture;
#[test]
#[cfg(feature = "pcap")]
fn ouster_create_packet() -> Result<()> {
let mut packets = vec![];
let mut cap = Capture::from_file("test_files/ouster_example.pcap")?;
cap.filter("udp")?;
while let Ok(packet) = cap.next() {
let lidar_packet = OusterPacket::from_pcap(&packet)?;
packets.push(lidar_packet);
}
let mut prev_timestamp = None;
for packet in packets.iter() {
let timestamp = packet.columns[0].timestamp;
if let Some(prev) = prev_timestamp {
assert!(timestamp > prev, "packets are not ordered by timestsamp");
}
prev_timestamp = Some(timestamp);
}
Ok(())
}
#[test]
#[cfg(feature = "pcap")]
fn ouster_pcd_converter() -> Result<()> {
let config = Config::from_path("test_files/ouster_example.json")?;
let pcd_converter = PointCloudConverter::from_config(config);
let mut cap = Capture::from_file("test_files/ouster_example.pcap")?;
cap.filter("udp")?;
while let Ok(packet) = cap.next() {
let lidar_packet = OusterPacket::from_pcap(&packet)?;
let points = pcd_converter.convert(lidar_packet)?;
assert!(points.len() as u16 == pcd_converter.columns_per_revolution());
}
Ok(())
}
#[test]
#[cfg(feature = "pcap")]
fn ouster_frame_converter() -> Result<()> {
let config = Config::from_path("test_files/ouster_example.json")?;
let mut frame_converter = FrameConverter::from_config(config);
let mut cap = Capture::from_file("test_files/ouster_example.pcap")?;
cap.filter("udp")?;
let mut frames = vec![];
while let Ok(packet) = cap.next() {
let lidar_packet = OusterPacket::from_pcap(&packet)?;
let new_frames = frame_converter.push_packet(&lidar_packet)?;
frames.extend(new_frames);
}
if let Some(frame) = frame_converter.finish() {
frames.push(frame);
}
let mut prev_frame_id_opt = None;
let mut prev_timestamp_opt = None;
for frame in frames {
if let Some(prev_frame_id) = prev_frame_id_opt {
assert!(prev_frame_id < frame.frame_id, "Frame ID is not ordered");
}
prev_frame_id_opt = Some(frame.frame_id);
let mut prev_measurement_id_opt = None;
for (measurement_id, timestamp) in frame.timestamps.iter() {
if let Some(prev_timestamp) = prev_timestamp_opt {
assert!(prev_timestamp < *timestamp, "Timestamp is not ordered");
}
prev_timestamp_opt = Some(*timestamp);
if let Some(prev_measurement_id) = prev_measurement_id_opt {
assert!(
prev_measurement_id < *measurement_id,
"Measurement ID is not ordered"
);
}
prev_measurement_id_opt = Some(*measurement_id);
}
}
Ok(())
}