1use super::{
4 consts::{
5 PUCK_HIRES_AZIMUTH_OFFSETS, PUCK_HIRES_ELEVAION_DEGREES, PUCK_HIRES_HORIZONTAL_OFFSETS,
6 PUCK_HIRES_VERTICAL_OFFSETS, PUCK_LITE_AZIMUTH_OFFSETS, PUCK_LITE_ELEVAION_DEGREES,
7 PUCK_LITE_HORIZONTAL_OFFSETS, PUCK_LITE_VERTICAL_OFFSETS, VLP_16_AZIMUTH_OFFSETS,
8 VLP_16_ELEVAION_DEGREES, VLP_16_HORIZONTAL_OFFSETS, VLP_16_VERTICAL_OFFSETS,
9 VLP_32C_AZIMUTH_OFFSETS, VLP_32C_ELEVAION_DEGREES, VLP_32C_HORIZONTAL_OFFSETS,
10 VLP_32C_VERTICAL_OFFSETS,
11 },
12 marker::{
13 DualReturn, DynamicModel, DynamicReturn, LastReturn, ModelMarker, ReturnTypeMarker,
14 StrongestReturn, Vlp16, Vlp32,
15 },
16 packet::ReturnMode,
17};
18use crate::common::*;
19
20pub use config_::*;
21pub use param_config::*;
22pub use params::*;
23
24mod config_ {
25 use super::*;
26
27 #[derive(Debug, Clone)]
29 pub struct Config<Model, ReturnType>
30 where
31 Model: ModelMarker,
32 ReturnType: ReturnTypeMarker,
33 {
34 pub model: Model,
35 pub lasers: Model::ParamArray,
36 pub return_type: ReturnType,
37 pub distance_resolution: Length,
38 }
39
40 #[allow(non_camel_case_types)]
41 pub type Vlp16_Strongest_Config = Config<Vlp16, StrongestReturn>;
42 #[allow(non_camel_case_types)]
43 pub type Vlp16_Last_Config = Config<Vlp16, LastReturn>;
44 #[allow(non_camel_case_types)]
45 pub type Vlp16_Dual_Config = Config<Vlp16, DualReturn>;
46 #[allow(non_camel_case_types)]
47 pub type Vlp16_Dynamic_Config = Config<Vlp16, DynamicReturn>;
48 #[allow(non_camel_case_types)]
49 pub type Vlp32_Strongest_Config = Config<Vlp32, StrongestReturn>;
50 #[allow(non_camel_case_types)]
51 pub type Vlp32_Last_Config = Config<Vlp32, LastReturn>;
52 #[allow(non_camel_case_types)]
53 pub type Vlp32_Dual_Config = Config<Vlp32, DualReturn>;
54 #[allow(non_camel_case_types)]
55 pub type Vlp32_Dynamic_Config = Config<Vlp32, DynamicReturn>;
56 #[allow(non_camel_case_types)]
57 pub type Dynamic_Config = Config<DynamicModel, DynamicReturn>;
58
59 impl<Model, ReturnType> Config<Model, ReturnType>
60 where
61 Model: ModelMarker,
62 ReturnType: ReturnTypeMarker,
63 {
64 pub fn into_dyn(self) -> Dynamic_Config {
65 let Self {
66 model,
67 lasers,
68 return_type,
69 distance_resolution,
70 } = self;
71
72 Dynamic_Config {
73 model: model.into_dynamic(),
74 lasers: Model::to_dynamic_params(lasers),
75 return_type: return_type.into_dynamic(),
76 distance_resolution,
77 }
78 }
79 }
80
81 impl Vlp16_Last_Config {
82 pub fn vlp_16_last_return() -> Self {
83 Config {
84 model: Vlp16,
85 lasers: vlp_16_laser_params(),
86 distance_resolution: Length::new::<millimeter>(VLP_16_DISTANCE_RESOLUTION_MILLIS),
87 return_type: LastReturn,
88 }
89 }
90
91 pub fn puck_hires_last_return() -> Self {
92 Config {
93 model: Vlp16,
94 lasers: puck_hires_laser_params(),
95 distance_resolution: Length::new::<millimeter>(
96 PUCK_HIRES_DISTANCE_RESOLUTION_MILLIS,
97 ),
98 return_type: LastReturn,
99 }
100 }
101
102 pub fn puck_lite_last_return() -> Self {
103 Config {
104 model: Vlp16,
105 lasers: puck_lite_laser_params(),
106 distance_resolution: Length::new::<millimeter>(
107 PUCK_LITE_DISTANCE_RESOLUTION_MILLIS,
108 ),
109 return_type: LastReturn,
110 }
111 }
112 }
113
114 impl Vlp16_Strongest_Config {
115 pub fn vlp_16_strongest_return() -> Self {
116 Config {
117 model: Vlp16,
118 lasers: vlp_16_laser_params(),
119 distance_resolution: Length::new::<millimeter>(VLP_16_DISTANCE_RESOLUTION_MILLIS),
120 return_type: StrongestReturn,
121 }
122 }
123
124 pub fn puck_hires_strongest_return() -> Self {
125 Config {
126 model: Vlp16,
127 lasers: puck_hires_laser_params(),
128 distance_resolution: Length::new::<millimeter>(
129 PUCK_HIRES_DISTANCE_RESOLUTION_MILLIS,
130 ),
131 return_type: StrongestReturn,
132 }
133 }
134
135 pub fn puck_lite_strongest_return() -> Self {
136 Config {
137 model: Vlp16,
138 lasers: puck_lite_laser_params(),
139 distance_resolution: Length::new::<millimeter>(
140 PUCK_LITE_DISTANCE_RESOLUTION_MILLIS,
141 ),
142 return_type: StrongestReturn,
143 }
144 }
145 }
146
147 impl Vlp16_Dual_Config {
148 pub fn vlp_16_dual_return() -> Self {
149 Config {
150 model: Vlp16,
151 lasers: vlp_16_laser_params(),
152 distance_resolution: Length::new::<millimeter>(VLP_16_DISTANCE_RESOLUTION_MILLIS),
153 return_type: DualReturn,
154 }
155 }
156
157 pub fn puck_hires_dual_return() -> Self {
158 Config {
159 model: Vlp16,
160 lasers: puck_hires_laser_params(),
161 distance_resolution: Length::new::<millimeter>(
162 PUCK_HIRES_DISTANCE_RESOLUTION_MILLIS,
163 ),
164 return_type: DualReturn,
165 }
166 }
167
168 pub fn puck_lite_dual_return() -> Self {
169 Config {
170 model: Vlp16,
171 lasers: puck_lite_laser_params(),
172 distance_resolution: Length::new::<millimeter>(
173 PUCK_LITE_DISTANCE_RESOLUTION_MILLIS,
174 ),
175 return_type: DualReturn,
176 }
177 }
178 }
179
180 impl Vlp16_Dynamic_Config {
181 pub fn vlp_16_dynamic_return(return_mode: ReturnMode) -> Self {
182 Config {
183 model: Vlp16,
184 lasers: vlp_16_laser_params(),
185 distance_resolution: Length::new::<millimeter>(VLP_16_DISTANCE_RESOLUTION_MILLIS),
186 return_type: DynamicReturn::from(return_mode),
187 }
188 }
189
190 pub fn puck_hires_dynamic_return(return_mode: ReturnMode) -> Self {
191 Config {
192 model: Vlp16,
193 lasers: puck_hires_laser_params(),
194 distance_resolution: Length::new::<millimeter>(
195 PUCK_HIRES_DISTANCE_RESOLUTION_MILLIS,
196 ),
197 return_type: DynamicReturn::from(return_mode),
198 }
199 }
200
201 pub fn puck_lite_dynamic_return(return_mode: ReturnMode) -> Self {
202 Config {
203 model: Vlp16,
204 lasers: puck_lite_laser_params(),
205 distance_resolution: Length::new::<millimeter>(
206 PUCK_LITE_DISTANCE_RESOLUTION_MILLIS,
207 ),
208 return_type: DynamicReturn::from(return_mode),
209 }
210 }
211 }
212
213 impl Vlp32_Last_Config {
214 pub fn vlp_32c_last_return() -> Self {
215 Config {
216 model: Vlp32,
217 lasers: vlp_32c_laser_params(),
218 distance_resolution: Length::new::<millimeter>(VLP_32C_DISTANCE_RESOLUTION_MILLIS),
219 return_type: LastReturn,
220 }
221 }
222 }
223
224 impl Vlp32_Strongest_Config {
225 pub fn vlp_32c_strongest_return() -> Self {
226 Config {
227 model: Vlp32,
228 lasers: vlp_32c_laser_params(),
229 distance_resolution: Length::new::<millimeter>(VLP_32C_DISTANCE_RESOLUTION_MILLIS),
230 return_type: StrongestReturn,
231 }
232 }
233 }
234
235 impl Vlp32_Dual_Config {
236 pub fn vlp_32c_dual_return() -> Self {
237 Config {
238 model: Vlp32,
239 lasers: vlp_32c_laser_params(),
240 distance_resolution: Length::new::<millimeter>(VLP_32C_DISTANCE_RESOLUTION_MILLIS),
241 return_type: DualReturn,
242 }
243 }
244 }
245
246 impl Vlp32_Dynamic_Config {
247 pub fn vlp_32c_dynamic_return(return_mode: ReturnMode) -> Self {
248 Config {
249 model: Vlp32,
250 lasers: vlp_32c_laser_params(),
251 distance_resolution: Length::new::<millimeter>(VLP_32C_DISTANCE_RESOLUTION_MILLIS),
252 return_type: DynamicReturn::from(return_mode),
253 }
254 }
255 }
256}
257
258mod params {
259 use super::*;
260
261 #[derive(Debug, Clone)]
262 pub struct LaserParameter {
263 pub elevation_angle: Angle,
264 pub azimuth_offset: Angle,
265 pub vertical_offset: Length,
266 pub horizontal_offset: Length,
267 }
268
269 pub const VLP_16_DISTANCE_RESOLUTION_MILLIS: f64 = 2.0;
270 pub const PUCK_HIRES_DISTANCE_RESOLUTION_MILLIS: f64 = 2.0;
271 pub const PUCK_LITE_DISTANCE_RESOLUTION_MILLIS: f64 = 2.0;
272 pub const VLP_32C_DISTANCE_RESOLUTION_MILLIS: f64 = 4.0;
273
274 pub fn vlp_16_laser_params() -> [LaserParameter; 16] {
275 let mut params: [MaybeUninit<LaserParameter>; 16] =
276 unsafe { MaybeUninit::uninit().assume_init() };
277 izip!(
278 params.iter_mut(),
279 VLP_16_ELEVAION_DEGREES.iter(),
280 VLP_16_VERTICAL_OFFSETS.iter(),
281 VLP_16_HORIZONTAL_OFFSETS.iter(),
282 VLP_16_AZIMUTH_OFFSETS.iter(),
283 )
284 .for_each(
285 |(param, &elevation_angle, &vertical_offset, &horizontal_offset, &azimuth_offset)| {
286 *param = MaybeUninit::new(LaserParameter {
287 elevation_angle: Angle::new::<degree>(elevation_angle),
288 vertical_offset: Length::new::<millimeter>(vertical_offset),
289 horizontal_offset: Length::new::<millimeter>(horizontal_offset),
290 azimuth_offset: Angle::new::<degree>(azimuth_offset),
291 });
292 },
293 );
294
295 unsafe { mem::transmute::<_, [LaserParameter; 16]>(params) }
296 }
297
298 pub fn puck_hires_laser_params() -> [LaserParameter; 16] {
299 let mut params: [MaybeUninit<LaserParameter>; 16] =
300 unsafe { MaybeUninit::uninit().assume_init() };
301 izip!(
302 params.iter_mut(),
303 PUCK_HIRES_ELEVAION_DEGREES.iter(),
304 PUCK_HIRES_VERTICAL_OFFSETS.iter(),
305 PUCK_HIRES_HORIZONTAL_OFFSETS.iter(),
306 PUCK_HIRES_AZIMUTH_OFFSETS.iter(),
307 )
308 .for_each(
309 |(param, &elevation_angle, &vertical_offset, &horizontal_offset, &azimuth_offset)| {
310 *param = MaybeUninit::new(LaserParameter {
311 elevation_angle: Angle::new::<degree>(elevation_angle),
312 vertical_offset: Length::new::<millimeter>(vertical_offset),
313 horizontal_offset: Length::new::<millimeter>(horizontal_offset),
314 azimuth_offset: Angle::new::<degree>(azimuth_offset),
315 });
316 },
317 );
318
319 unsafe { mem::transmute::<_, [LaserParameter; 16]>(params) }
320 }
321
322 pub fn puck_lite_laser_params() -> [LaserParameter; 16] {
323 let mut params: [MaybeUninit<LaserParameter>; 16] =
324 unsafe { MaybeUninit::uninit().assume_init() };
325 izip!(
326 params.iter_mut(),
327 PUCK_LITE_ELEVAION_DEGREES.iter(),
328 PUCK_LITE_VERTICAL_OFFSETS.iter(),
329 PUCK_LITE_HORIZONTAL_OFFSETS.iter(),
330 PUCK_LITE_AZIMUTH_OFFSETS.iter(),
331 )
332 .for_each(
333 |(param, &elevation_angle, &vertical_offset, &horizontal_offset, &azimuth_offset)| {
334 *param = MaybeUninit::new(LaserParameter {
335 elevation_angle: Angle::new::<degree>(elevation_angle),
336 vertical_offset: Length::new::<millimeter>(vertical_offset),
337 horizontal_offset: Length::new::<millimeter>(horizontal_offset),
338 azimuth_offset: Angle::new::<degree>(azimuth_offset),
339 });
340 },
341 );
342
343 unsafe { mem::transmute::<_, [LaserParameter; 16]>(params) }
344 }
345
346 pub fn vlp_32c_laser_params() -> [LaserParameter; 32] {
347 let mut params: [MaybeUninit<LaserParameter>; 32] =
348 unsafe { MaybeUninit::uninit().assume_init() };
349 izip!(
350 params.iter_mut(),
351 VLP_32C_ELEVAION_DEGREES.iter(),
352 VLP_32C_VERTICAL_OFFSETS.iter(),
353 VLP_32C_HORIZONTAL_OFFSETS.iter(),
354 VLP_32C_AZIMUTH_OFFSETS.iter(),
355 )
356 .for_each(
357 |(param, &elevation_angle, &vertical_offset, &horizontal_offset, &azimuth_offset)| {
358 *param = MaybeUninit::new(LaserParameter {
359 elevation_angle: Angle::new::<degree>(elevation_angle),
360 vertical_offset: Length::new::<millimeter>(vertical_offset),
361 horizontal_offset: Length::new::<millimeter>(horizontal_offset),
362 azimuth_offset: Angle::new::<degree>(azimuth_offset),
363 });
364 },
365 );
366
367 unsafe { mem::transmute::<_, [LaserParameter; 32]>(params) }
368 }
369}
370
371mod param_config {
372 use super::*;
373
374 #[derive(Debug, Clone, Serialize, Deserialize)]
375 pub struct ParamsConfig {
376 lasers: Vec<LaserConfig>,
377 num_lasers: usize,
378 distance_resolution: f64,
379 }
380
381 #[derive(Debug, Clone, Serialize, Deserialize)]
382 pub struct LaserConfig {
383 pub dist_correction: f64,
384 pub dist_correction_x: f64,
385 pub dist_correction_y: f64,
386 pub focal_distance: f64,
387 pub focal_slope: f64,
388 pub horiz_offset_correction: Option<f64>,
389 pub laser_id: usize,
390 pub rot_correction: f64,
391 pub vert_correction: f64,
392 pub vert_offset_correction: f64,
393 }
394
395 impl ParamsConfig {
396 pub fn load<P>(path: P) -> Result<Self>
397 where
398 P: AsRef<Path>,
399 {
400 let mut reader = BufReader::new(File::open(path)?);
401 let config = Self::from_reader(&mut reader)?;
402 Ok(config)
403 }
404
405 pub fn from_reader<R>(reader: &mut R) -> Result<Self>
406 where
407 R: Read,
408 {
409 let mut text = String::new();
410 reader.read_to_string(&mut text)?;
411 let config = Self::from_str(&text)?;
412 Ok(config)
413 }
414 }
415
416 impl FromStr for ParamsConfig {
417 type Err = Error;
418
419 fn from_str(text: &str) -> Result<Self, Self::Err> {
420 let config: Self = serde_yaml::from_str(text)?;
421 ensure!(
422 config.distance_resolution > 0.0,
423 "distance_resolution must be positive"
424 );
425 ensure!(
426 config.num_lasers == config.lasers.len(),
427 "the number of element in lasers field does not match num_layers"
428 );
429 ensure!(
430 {
431 config
432 .lasers
433 .iter()
434 .enumerate()
435 .all(|(idx, params)| idx == params.laser_id)
436 },
437 "the laser_id in lasers field must be consecutively counted from 1"
438 );
439 Ok(config)
440 }
441 }
442}
443
444#[cfg(test)]
445mod tests {
446 use super::*;
447 use anyhow::Result;
448
449 #[test]
450 fn buildin_config_test() -> Result<()> {
451 let _: Vlp16_Last_Config = Config::vlp_16_last_return();
452 let _: Vlp16_Strongest_Config = Config::vlp_16_strongest_return();
453 let _: Vlp16_Dual_Config = Config::vlp_16_dual_return();
454
455 let _: Vlp16_Last_Config = Config::puck_hires_last_return();
456 let _: Vlp16_Strongest_Config = Config::puck_hires_strongest_return();
457 let _: Vlp16_Dual_Config = Config::puck_hires_dual_return();
458
459 let _: Vlp16_Last_Config = Config::puck_lite_last_return();
460 let _: Vlp16_Strongest_Config = Config::puck_lite_strongest_return();
461 let _: Vlp16_Dual_Config = Config::puck_lite_dual_return();
462
463 let _: Vlp32_Last_Config = Config::vlp_32c_last_return();
464 let _: Vlp32_Strongest_Config = Config::vlp_32c_strongest_return();
465 let _: Vlp32_Dual_Config = Config::vlp_32c_dual_return();
466
467 Ok(())
468 }
469
470 #[test]
471 fn load_yaml_params_test() -> Result<()> {
472 ParamsConfig::from_str(include_str!("params/32db.yaml"))?;
473 ParamsConfig::from_str(include_str!("params/64e_s2.1-sztaki.yaml"))?;
474 ParamsConfig::from_str(include_str!("params/64e_s3-xiesc.yaml"))?;
475 ParamsConfig::from_str(include_str!("params/64e_utexas.yaml"))?;
476 ParamsConfig::from_str(include_str!("params/VeloView-VLP-32C.yaml"))?;
477 ParamsConfig::from_str(include_str!("params/VLP16db.yaml"))?;
478 ParamsConfig::from_str(include_str!("params/VLP16_hires_db.yaml"))?;
479 Ok(())
480 }
481}