lidar_utils/ouster/
client.rs

1//! Tools to work with TCP API on Ouster sensors.
2
3use super::{
4    consts::PIXELS_PER_COLUMN,
5    enums::{LidarMode, MultipurposeIoMode, NmeaBaudRate, OnOffMode, Polarity, TimestampMode},
6};
7use crate::common::*;
8
9// TODO: This workaround handles large array for serde.
10//       We'll remove is it once the const generics is introduced.
11big_array! { BigArray; }
12
13#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
14pub struct ConfigText {
15    pub timestamp_mode: TimestampMode,
16    pub multipurpose_io_mode: MultipurposeIoMode,
17    pub lidar_mode: LidarMode,
18    pub sync_pulse_in_polarity: Polarity,
19    pub nmea_in_polarity: Polarity,
20    pub sync_pulse_out_polarity: Polarity,
21    pub udp_ip: Ipv4Addr,
22    #[serde(with = "serde_bool_to_int")]
23    pub nmea_ignore_valid_char: bool,
24    #[serde(with = "serde_bool_to_int")]
25    pub auto_start_flag: bool,
26    pub sync_pulse_out_pulse_width: u64,
27    pub nmea_baud_rate: NmeaBaudRate,
28    pub sync_pulse_out_angle: u64,
29    pub sync_pulse_out_frequency: u64,
30    pub udp_port_imu: u16,
31    pub udp_port_lidar: u16,
32    pub azimuth_window: [u64; 2],
33}
34
35#[derive(Clone, PartialEq, Eq, Hash, Serialize, Deserialize, Derivative)]
36#[derivative(Debug)]
37pub struct BeamIntrinsics {
38    #[serde(with = "BigArray")]
39    #[derivative(Debug(format_with = "self::large_array_fmt"))]
40    pub beam_altitude_angles: [R64; PIXELS_PER_COLUMN],
41    #[serde(with = "BigArray")]
42    #[derivative(Debug(format_with = "self::large_array_fmt"))]
43    pub beam_azimuth_angles: [R64; PIXELS_PER_COLUMN],
44}
45
46#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
47pub struct LidarIntrinsics {
48    pub lidar_to_sensor_transform: [R64; 16],
49}
50
51#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
52pub struct ImuIntrinsics {
53    pub imu_to_sensor_transform: [R64; 16],
54}
55
56#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
57pub struct TimeInfo {
58    pub timestamp: TimestampInfo,
59    pub sync_pulse_in: SyncPulseInInfo,
60    pub multipurpose_io: MultiPurposeIo,
61}
62
63#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
64pub struct MultiPurposeIo {
65    pub mode: OnOffMode,
66    pub sync_pulse_out: SyncPulseOutInfo,
67    pub nmea: NmeaInfo,
68}
69
70#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
71pub struct SyncPulseInInfo {
72    pub diagnostics: SyncPulseInDiagnosticsInfo,
73    pub polarity: Polarity,
74    #[serde(with = "serde_bool_to_int")]
75    pub locked: bool,
76}
77
78#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
79pub struct NmeaInfo {
80    pub polarity: Polarity,
81    pub baud_rate: NmeaBaudRate,
82    pub diagnostics: NmeaDiagnosticsInfo,
83    pub leap_seconds: u64,
84    #[serde(with = "serde_bool_to_int")]
85    pub ignore_valid_char: bool,
86    #[serde(with = "serde_bool_to_int")]
87    pub locked: bool,
88}
89
90#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
91pub struct SyncPulseOutInfo {
92    pub frequency_hz: u64,
93    pub angle_deg: u64,
94    pub pulse_width_ms: u64,
95    pub polarity: Polarity,
96}
97
98#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
99pub struct TimestampInfo {
100    pub time_options: TimeOptionsInfo,
101    pub mode: TimestampMode,
102    pub time: R64,
103}
104
105#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
106pub struct SyncPulseInDiagnosticsInfo {
107    pub count_unfiltered: u64,
108    pub last_period_nsec: u64,
109    pub count: u64,
110}
111
112#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
113pub struct NmeaDiagnosticsInfo {
114    pub io_checks: NmeaIoChecksInfo,
115    pub decoding: NmeaDecodingInfo,
116}
117
118#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
119pub struct TimeOptionsInfo {
120    pub ptp_1588: u64,
121    #[serde(with = "serde_bool_to_int")]
122    pub sync_pulse_in: bool,
123    pub internal_osc: u64,
124}
125
126#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
127pub struct NmeaIoChecksInfo {
128    pub bit_count: u64,
129    pub start_char_count: u64,
130    pub bit_count_unfilterd: u64,
131    pub char_count: u64,
132}
133
134#[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
135pub struct NmeaDecodingInfo {
136    pub not_valid_count: u64,
137    pub last_read_message: String,
138    pub utc_decoded_count: u64,
139    pub date_decoded_count: u64,
140}
141
142#[derive(Debug)]
143pub struct CommandClient {
144    reader: Lines<BufReader<TcpStream>>,
145    writer: LineWriter<TcpStream>,
146}
147
148impl CommandClient {
149    pub fn connect<A>(address: A, timeout: Option<Duration>) -> Result<CommandClient>
150    where
151        A: ToSocketAddrs,
152    {
153        let stream = TcpStream::connect(&address)?;
154        stream.set_read_timeout(timeout)?;
155        stream.set_write_timeout(timeout)?;
156
157        let reader = BufReader::new(stream.try_clone()?).lines();
158        let writer = LineWriter::new(stream);
159        let client = CommandClient { reader, writer };
160        Ok(client)
161    }
162
163    pub fn get_config_txt(&mut self) -> Result<ConfigText> {
164        self.writer.write_all(b"get_config_txt\n")?;
165        let line = self
166            .reader
167            .next()
168            .ok_or_else(|| format_err!("Unexpected end of stream"))??;
169        let config = serde_json::from_str(&line)?;
170        Ok(config)
171    }
172
173    pub fn get_time_info(&mut self) -> Result<TimeInfo> {
174        self.writer.write_all(b"get_time_info\n")?;
175        let line = self
176            .reader
177            .next()
178            .ok_or_else(|| format_err!("Unexpected end of stream"))??;
179        let config = serde_json::from_str(&line)?;
180        Ok(config)
181    }
182
183    pub fn get_lidar_intrinsics(&mut self) -> Result<LidarIntrinsics> {
184        self.writer.write_all(b"get_lidar_intrinsics\n")?;
185        let line = self
186            .reader
187            .next()
188            .ok_or_else(|| format_err!("Unexpected end of stream"))??;
189        let config = serde_json::from_str(&line)?;
190        Ok(config)
191    }
192
193    pub fn get_imu_intrinsics(&mut self) -> Result<ImuIntrinsics> {
194        self.writer.write_all(b"get_imu_intrinsics\n")?;
195        let line = self
196            .reader
197            .next()
198            .ok_or_else(|| format_err!("Unexpected end of stream"))??;
199        let config = serde_json::from_str(&line)?;
200        Ok(config)
201    }
202
203    pub fn get_beam_intrinsics(&mut self) -> Result<BeamIntrinsics> {
204        self.writer.write_all(b"get_beam_intrinsics\n")?;
205        let line = self
206            .reader
207            .next()
208            .ok_or_else(|| format_err!("Unexpected end of stream"))??;
209        let config = serde_json::from_str(&line)?;
210        Ok(config)
211    }
212
213    pub fn reinitialize(mut self) -> Result<()> {
214        self.writer.write_all(b"reinitialize\n")?;
215        let line = self
216            .reader
217            .next()
218            .ok_or_else(|| format_err!("Unexpected end of stream"))??;
219        ensure!(line == "reinitialize", "Unexpected response {:?}", line);
220        Ok(())
221    }
222
223    pub fn write_config_txt(&mut self) -> Result<()> {
224        self.writer.write_all(b"write_config_txt\n")?;
225        let line = self
226            .reader
227            .next()
228            .ok_or_else(|| format_err!("Unexpected end of stream"))??;
229        ensure!(line == "write_config_txt", "Unexpected response {:?}", line);
230        Ok(())
231    }
232
233    pub fn set_udp_ip(&mut self, ip: Ipv4Addr) -> Result<()> {
234        self.set_config_param("udp_ip", ip)?;
235        Ok(())
236    }
237
238    pub fn set_udp_port_lidar(&mut self, port: u16) -> Result<()> {
239        self.set_config_param("udp_port_lidar", port)?;
240        Ok(())
241    }
242
243    pub fn set_udp_port_imu(&mut self, port: u16) -> Result<()> {
244        self.set_config_param("udp_port_imu", port)?;
245        Ok(())
246    }
247
248    pub fn set_lidar_mode(&mut self, mode: LidarMode) -> Result<()> {
249        self.set_config_param("lidar_mode", mode)?;
250        Ok(())
251    }
252
253    pub fn set_timestamp_mode(&mut self, mode: TimestampMode) -> Result<()> {
254        self.set_config_param("timestamp_mode", mode)?;
255        Ok(())
256    }
257
258    pub fn set_sync_pulse_in_polarity(&mut self, polarity: Polarity) -> Result<()> {
259        self.set_config_param("sync_pulse_in_polarity", polarity)?;
260        Ok(())
261    }
262
263    pub fn set_nmea_in_polarity(&mut self, polarity: Polarity) -> Result<()> {
264        self.set_config_param("nmea_in_polarity", polarity)?;
265        Ok(())
266    }
267
268    fn set_config_param<T: Display>(&mut self, param: &str, arg: T) -> Result<()> {
269        let command = format!("set_config_param {} {}\n", param, arg);
270        self.writer.write_all(command.as_bytes())?;
271        let line = self
272            .reader
273            .next()
274            .ok_or_else(|| format_err!("Unexpected end of stream"))??;
275        ensure!(line == "set_config_param", "Unexpected response {:?}", line);
276        Ok(())
277    }
278}
279
280mod serde_bool_to_int {
281    use super::*;
282
283    pub fn serialize<S>(value: &bool, serializer: S) -> Result<S::Ok, S::Error>
284    where
285        S: Serializer,
286    {
287        match value {
288            true => serializer.serialize_u64(1),
289            false => serializer.serialize_u64(0),
290        }
291    }
292
293    pub fn deserialize<'de, D>(deserializer: D) -> Result<bool, D::Error>
294    where
295        D: Deserializer<'de>,
296    {
297        match u64::deserialize(deserializer)? {
298            1 => Ok(true),
299            0 => Ok(false),
300            other => {
301                use serde::de::{Error, Unexpected};
302                let error = Error::invalid_value(Unexpected::Unsigned(other), &"Expect 0 or 1");
303                Err(error)
304            }
305        }
306    }
307}
308
309fn large_array_fmt<T: Debug>(
310    array: &[T; PIXELS_PER_COLUMN],
311    formatter: &mut Formatter,
312) -> Result<(), fmt::Error> {
313    write!(formatter, "{:?}", &array[..])
314}
lidar_utils/ouster/client.rs

lidar_utils/ouster/
client.rs
