#![cfg(feature = "ouster-api-test")]
use anyhow::Result;
use lidar_utils::ouster::{client::CommandClient, packet::Packet as OusterPacket};
use log::warn;
use serde::Deserialize;
use std::{
fs::File,
io::prelude::*,
net::{Ipv4Addr, SocketAddr, UdpSocket},
path::PathBuf,
time::{Duration, Instant},
};
const MAX_UDP_PACKET_SIZE: usize = 65507;
#[derive(Deserialize, Clone, Debug)]
struct OusterClientTestConfig {
lidar_addr: Ipv4Addr,
listen_addr: Ipv4Addr,
timeout: u64,
}
#[test]
fn ouster_client_test() -> Result<()> {
let config: OusterClientTestConfig = {
let path = PathBuf::from(env!("CARGO_MANIFEST_DIR"))
.join("test_files")
.join("ouster_client_test.toml");
let mut config_file = File::open(path)?;
let mut config_str = String::new();
config_file.read_to_string(&mut config_str)?;
toml::from_str(&config_str)?
};
let lidar_addr = SocketAddr::new(config.lidar_addr.into(), 7501);
let timeout = Duration::from_secs(config.timeout);
let mut client = CommandClient::connect(lidar_addr, Some(timeout))?;
let config_txt = client.get_config_txt()?;
let time_info = client.get_time_info()?;
let beam_intrinsics = client.get_beam_intrinsics()?;
let lidar_intrinsics = client.get_lidar_intrinsics()?;
let imu_intrinsics = client.get_imu_intrinsics()?;
dbg!(&config_txt);
dbg!(&time_info);
dbg!(&beam_intrinsics);
dbg!(&lidar_intrinsics);
dbg!(&imu_intrinsics);
client.set_udp_ip(config.listen_addr)?;
client.reinitialize()?;
std::thread::sleep(Duration::from_secs(10));
let bind_addr = SocketAddr::from((config.listen_addr, config_txt.udp_port_lidar));
let socket = UdpSocket::bind(bind_addr)?;
socket.set_read_timeout(Some(timeout))?;
let packet_size = std::mem::size_of::<OusterPacket>();
let instant = Instant::now();
loop {
let mut buf = [0; MAX_UDP_PACKET_SIZE];
let (read_size, peer_addr) = socket.recv_from(&mut buf)?;
if lidar_addr.ip() != peer_addr.ip() || packet_size != read_size {
continue;
}
let packet_buf = &buf[..packet_size];
match OusterPacket::from_slice(packet_buf) {
Ok(_packet) => {
println!(
"received packet from LIDAR in {} milliseconds",
instant.elapsed().as_secs()
);
break;
}
Err(error) => {
warn!(
"packet decoding error: {:?}. Proceed to next packet.",
error
);
continue;
}
}
}
Ok(())
}