1use super::impls;
2use crate::{
3 common::*,
4 velodyne::{
5 config::{
6 Config, Dynamic_Config, LaserParameter, Vlp16_Dual_Config, Vlp16_Dynamic_Config,
7 Vlp16_Last_Config, Vlp16_Strongest_Config, Vlp32_Dual_Config, Vlp32_Dynamic_Config,
8 Vlp32_Last_Config, Vlp32_Strongest_Config,
9 },
10 marker::{
11 DualReturn, DynamicModel, DynamicReturn, LastReturn, ModelMarker, ReturnTypeMarker,
12 StrongestReturn, Vlp16, Vlp32,
13 },
14 packet::{Block, DataPacket, ReturnMode},
15 point::{DualReturnPoint, DynamicReturnPoints, SingleReturnPoint},
16 },
17};
18
19pub use converter_impls::*;
20pub use definition::*;
21
22mod definition {
23 use super::*;
24
25 pub trait PointCloudConverter<Model, ReturnType>
27 where
28 Model: ModelMarker,
29 ReturnType: ReturnTypeMarker,
30 {
31 type Output;
32
33 fn from_config(config: Config<Model, ReturnType>) -> Self;
35
36 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
38 where
39 P: Borrow<DataPacket>;
40 }
41
42 #[derive(Debug)]
43 pub(crate) enum LastBlock {
44 Single(Option<(Time, Block)>),
45 Dual(Option<(Time, Block, Block)>),
46 }
47
48 impl LastBlock {
49 pub fn new(return_type: DynamicReturn) -> Self {
50 match return_type {
51 DynamicReturn::LastReturn | DynamicReturn::StrongestReturn => Self::Single(None),
52 DynamicReturn::DualReturn => Self::Dual(None),
53 }
54 }
55
56 pub fn single(&mut self) -> &mut Option<(Time, Block)> {
57 match self {
58 Self::Single(last_block) => last_block,
59 _ => unreachable!(),
60 }
61 }
62
63 pub fn dual(&mut self) -> &mut Option<(Time, Block, Block)> {
64 match self {
65 Self::Dual(last_block) => last_block,
66 _ => unreachable!(),
67 }
68 }
69 }
70
71 #[derive(Debug)]
72 #[allow(non_camel_case_types)]
73 pub struct Dynamic_PcdConverter {
74 pub(crate) model: DynamicModel,
75 pub(crate) return_type: DynamicReturn,
76 pub(crate) lasers: Vec<LaserParameter>,
77 pub(crate) distance_resolution: Length,
78 pub(crate) last_block: LastBlock,
79 }
80
81 #[derive(Debug)]
82 #[allow(non_camel_case_types)]
83 pub struct Vlp16_Strongest_PcdConverter {
84 pub(crate) lasers: [LaserParameter; 16],
85 pub(crate) distance_resolution: Length,
86 pub(crate) last_block: Option<(Time, Block)>,
87 }
88
89 #[derive(Debug)]
90 #[allow(non_camel_case_types)]
91 pub struct Vlp16_Last_PcdConverter {
92 pub(crate) lasers: [LaserParameter; 16],
93 pub(crate) distance_resolution: Length,
94 pub(crate) last_block: Option<(Time, Block)>,
95 }
96
97 #[derive(Debug)]
98 #[allow(non_camel_case_types)]
99 pub struct Vlp16_Dual_PcdConverter {
100 pub(crate) lasers: [LaserParameter; 16],
101 pub(crate) distance_resolution: Length,
102 pub(crate) last_block: Option<(Time, Block, Block)>,
103 }
104
105 #[derive(Debug)]
106 #[allow(non_camel_case_types)]
107 pub struct Vlp16_Dynamic_PcdConverter {
108 pub(crate) return_type: DynamicReturn,
109 pub(crate) lasers: [LaserParameter; 16],
110 pub(crate) distance_resolution: Length,
111 pub(crate) last_block: LastBlock,
112 }
113
114 #[derive(Debug)]
115 #[allow(non_camel_case_types)]
116 pub struct Vlp32_Strongest_PcdConverter {
117 pub(crate) lasers: [LaserParameter; 32],
118 pub(crate) distance_resolution: Length,
119 pub(crate) last_block: Option<(Time, Block)>,
120 }
121
122 #[derive(Debug)]
123 #[allow(non_camel_case_types)]
124 pub struct Vlp32_Last_PcdConverter {
125 pub(crate) lasers: [LaserParameter; 32],
126 pub(crate) distance_resolution: Length,
127 pub(crate) last_block: Option<(Time, Block)>,
128 }
129
130 #[derive(Debug)]
131 #[allow(non_camel_case_types)]
132 pub struct Vlp32_Dual_PcdConverter {
133 pub(crate) lasers: [LaserParameter; 32],
134 pub(crate) distance_resolution: Length,
135 pub(crate) last_block: Option<(Time, Block, Block)>,
136 }
137
138 #[derive(Debug)]
139 #[allow(non_camel_case_types)]
140 pub struct Vlp32_Dynamic_PcdConverter {
141 pub(crate) return_type: DynamicReturn,
142 pub(crate) lasers: [LaserParameter; 32],
143 pub(crate) distance_resolution: Length,
144 pub(crate) last_block: LastBlock,
145 }
146}
147
148mod converter_impls {
149 use super::*;
150
151 impl PointCloudConverter<Vlp16, StrongestReturn> for Vlp16_Strongest_PcdConverter {
152 type Output = Vec<SingleReturnPoint>;
153
154 fn from_config(config: Vlp16_Strongest_Config) -> Self {
155 let Config {
156 lasers,
157 distance_resolution,
158 ..
159 } = config;
160
161 Self {
162 lasers,
163 distance_resolution,
164 last_block: None,
165 }
166 }
167
168 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
169 where
170 P: Borrow<DataPacket>,
171 {
172 let Self {
173 ref lasers,
174 distance_resolution,
175 ref mut last_block,
176 } = *self;
177
178 let packet = packet.borrow();
179 ensure!(
180 packet.return_mode == ReturnMode::StrongestReturn,
181 "return mode does not match"
182 );
183 Ok(impls::convert_single_return_16_channel(
184 lasers,
185 distance_resolution,
186 last_block,
187 packet,
188 ))
189 }
190 }
191
192 impl PointCloudConverter<Vlp16, LastReturn> for Vlp16_Last_PcdConverter {
193 type Output = Vec<SingleReturnPoint>;
194
195 fn from_config(config: Vlp16_Last_Config) -> Self {
196 let Config {
197 lasers,
198 distance_resolution,
199 ..
200 } = config;
201
202 Self {
203 lasers,
204 distance_resolution,
205 last_block: None,
206 }
207 }
208
209 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
210 where
211 P: Borrow<DataPacket>,
212 {
213 let Self {
214 ref lasers,
215 distance_resolution,
216 ref mut last_block,
217 } = *self;
218
219 let packet = packet.borrow();
220 ensure!(
221 packet.return_mode == ReturnMode::LastReturn,
222 "return mode does not match"
223 );
224 Ok(impls::convert_single_return_16_channel(
225 lasers,
226 distance_resolution,
227 last_block,
228 packet,
229 ))
230 }
231 }
232
233 impl PointCloudConverter<Vlp16, DualReturn> for Vlp16_Dual_PcdConverter {
234 type Output = Vec<DualReturnPoint>;
235
236 fn from_config(config: Vlp16_Dual_Config) -> Self {
237 let Config {
238 lasers,
239 distance_resolution,
240 ..
241 } = config;
242
243 Self {
244 lasers,
245 distance_resolution,
246 last_block: None,
247 }
248 }
249
250 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
251 where
252 P: Borrow<DataPacket>,
253 {
254 let Self {
255 ref lasers,
256 distance_resolution,
257 ref mut last_block,
258 } = *self;
259
260 let packet = packet.borrow();
261 ensure!(
262 packet.return_mode == ReturnMode::DualReturn,
263 "return mode does not match"
264 );
265 Ok(impls::convert_dual_return_16_channel(
266 lasers,
267 distance_resolution,
268 last_block,
269 packet,
270 ))
271 }
272 }
273
274 impl PointCloudConverter<Vlp16, DynamicReturn> for Vlp16_Dynamic_PcdConverter {
275 type Output = DynamicReturnPoints;
276
277 fn from_config(config: Vlp16_Dynamic_Config) -> Self {
278 let Config {
279 lasers,
280 return_type,
281 distance_resolution,
282 ..
283 } = config;
284
285 Self {
286 lasers,
287 return_type,
288 distance_resolution,
289 last_block: LastBlock::new(return_type),
290 }
291 }
292
293 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
294 where
295 P: Borrow<DataPacket>,
296 {
297 let Self {
298 return_type,
299 ref lasers,
300 distance_resolution,
301 ref mut last_block,
302 } = *self;
303
304 let packet = packet.borrow();
305
306 let points: DynamicReturnPoints = match return_type {
307 DynamicReturn::LastReturn | DynamicReturn::StrongestReturn => {
308 impls::convert_single_return_16_channel(
309 lasers,
310 distance_resolution,
311 last_block.single(),
312 packet,
313 )
314 .into()
315 }
316 DynamicReturn::DualReturn => impls::convert_dual_return_16_channel(
317 lasers,
318 distance_resolution,
319 last_block.dual(),
320 packet,
321 )
322 .into(),
323 };
324
325 Ok(points)
326 }
327 }
328
329 impl PointCloudConverter<Vlp32, StrongestReturn> for Vlp32_Strongest_PcdConverter {
330 type Output = Vec<SingleReturnPoint>;
331
332 fn from_config(config: Vlp32_Strongest_Config) -> Self {
333 let Config {
334 lasers,
335 distance_resolution,
336 ..
337 } = config;
338
339 Self {
340 lasers,
341 distance_resolution,
342 last_block: None,
343 }
344 }
345
346 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
347 where
348 P: Borrow<DataPacket>,
349 {
350 let Self {
351 ref lasers,
352 distance_resolution,
353 ref mut last_block,
354 } = *self;
355
356 let packet = packet.borrow();
357 ensure!(
358 packet.return_mode == ReturnMode::StrongestReturn,
359 "return mode does not match"
360 );
361 Ok(impls::convert_single_return_32_channel(
362 lasers,
363 distance_resolution,
364 last_block,
365 packet,
366 ))
367 }
368 }
369
370 impl PointCloudConverter<Vlp32, LastReturn> for Vlp32_Last_PcdConverter {
371 type Output = Vec<SingleReturnPoint>;
372
373 fn from_config(config: Vlp32_Last_Config) -> Self {
374 let Config {
375 lasers,
376 distance_resolution,
377 ..
378 } = config;
379
380 Self {
381 lasers,
382 distance_resolution,
383 last_block: None,
384 }
385 }
386
387 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
388 where
389 P: Borrow<DataPacket>,
390 {
391 let Self {
392 ref lasers,
393 distance_resolution,
394 ref mut last_block,
395 } = *self;
396
397 let packet = packet.borrow();
398 ensure!(
399 packet.return_mode == ReturnMode::LastReturn,
400 "return mode does not match"
401 );
402 Ok(impls::convert_single_return_32_channel(
403 lasers,
404 distance_resolution,
405 last_block,
406 packet,
407 ))
408 }
409 }
410
411 impl PointCloudConverter<Vlp32, DualReturn> for Vlp32_Dual_PcdConverter {
412 type Output = Vec<DualReturnPoint>;
413
414 fn from_config(config: Vlp32_Dual_Config) -> Self {
415 let Config {
416 lasers,
417 distance_resolution,
418 ..
419 } = config;
420
421 Self {
422 lasers,
423 distance_resolution,
424 last_block: None,
425 }
426 }
427
428 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
429 where
430 P: Borrow<DataPacket>,
431 {
432 let Self {
433 ref lasers,
434 distance_resolution,
435 ref mut last_block,
436 } = *self;
437
438 let packet = packet.borrow();
439 ensure!(
440 packet.return_mode == ReturnMode::DualReturn,
441 "return mode does not match"
442 );
443 Ok(impls::convert_dual_return_32_channel(
444 lasers,
445 distance_resolution,
446 last_block,
447 packet,
448 ))
449 }
450 }
451
452 impl PointCloudConverter<Vlp32, DynamicReturn> for Vlp32_Dynamic_PcdConverter {
453 type Output = DynamicReturnPoints;
454
455 fn from_config(config: Vlp32_Dynamic_Config) -> Self {
456 let Config {
457 lasers,
458 return_type,
459 distance_resolution,
460 ..
461 } = config;
462
463 Self {
464 lasers,
465 return_type,
466 distance_resolution,
467 last_block: LastBlock::new(return_type),
468 }
469 }
470
471 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
472 where
473 P: Borrow<DataPacket>,
474 {
475 let Self {
476 return_type,
477 ref lasers,
478 distance_resolution,
479 ref mut last_block,
480 } = *self;
481
482 let packet = packet.borrow();
483
484 let points: DynamicReturnPoints = match return_type {
485 DynamicReturn::LastReturn | DynamicReturn::StrongestReturn => {
486 impls::convert_single_return_32_channel(
487 lasers,
488 distance_resolution,
489 last_block.single(),
490 packet,
491 )
492 .into()
493 }
494 DynamicReturn::DualReturn => impls::convert_dual_return_32_channel(
495 lasers,
496 distance_resolution,
497 last_block.dual(),
498 packet,
499 )
500 .into(),
501 };
502
503 Ok(points)
504 }
505 }
506
507 impl PointCloudConverter<DynamicModel, DynamicReturn> for Dynamic_PcdConverter {
508 type Output = DynamicReturnPoints;
509
510 fn from_config(config: Dynamic_Config) -> Self {
511 let Config {
512 model,
513 lasers,
514 return_type,
515 distance_resolution,
516 ..
517 } = config;
518
519 Self {
520 model,
521 lasers,
522 return_type,
523 distance_resolution,
524 last_block: LastBlock::new(return_type),
525 }
526 }
527
528 fn convert<P>(&mut self, packet: P) -> Result<Self::Output>
529 where
530 P: Borrow<DataPacket>,
531 {
532 let Self {
533 model,
534 return_type,
535 ref lasers,
536 distance_resolution,
537 ref mut last_block,
538 } = *self;
539
540 let packet = packet.borrow();
541
542 let points: DynamicReturnPoints = match (model, return_type) {
543 (DynamicModel::Vlp16, DynamicReturn::LastReturn)
544 | (DynamicModel::Vlp16, DynamicReturn::StrongestReturn) => {
545 let lasers: &[_; 16] = lasers.as_slice().try_into().unwrap();
546 impls::convert_single_return_16_channel(
547 lasers,
548 distance_resolution,
549 last_block.single(),
550 packet,
551 )
552 .into()
553 }
554 (DynamicModel::Vlp16, DynamicReturn::DualReturn) => {
555 let lasers: &[_; 16] = lasers.as_slice().try_into().unwrap();
556 impls::convert_dual_return_16_channel(
557 lasers,
558 distance_resolution,
559 last_block.dual(),
560 packet,
561 )
562 .into()
563 }
564 (DynamicModel::Vlp32, DynamicReturn::LastReturn)
565 | (DynamicModel::Vlp32, DynamicReturn::StrongestReturn) => {
566 let lasers: &[_; 32] = lasers.as_slice().try_into().unwrap();
567 impls::convert_single_return_32_channel(
568 lasers,
569 distance_resolution,
570 last_block.single(),
571 packet,
572 )
573 .into()
574 }
575 (DynamicModel::Vlp32, DynamicReturn::DualReturn) => {
576 let lasers: &[_; 32] = lasers.as_slice().try_into().unwrap();
577 impls::convert_dual_return_32_channel(
578 lasers,
579 distance_resolution,
580 last_block.dual(),
581 packet,
582 )
583 .into()
584 }
585 };
586
587 Ok(points)
588 }
589 }
590}