1use serde::{Deserialize, Serialize};
10
11use crate::reception::exchange::mobile::Mobile;
12use crate::reception::exchange::mobile_perceived_object::MobilePerceivedObject;
13use crate::reception::exchange::perceived_object::PerceivedObject;
14use crate::reception::exchange::{PositionConfidence, ReferencePosition};
15use crate::reception::typed::Typed;
16
17#[serde_with::skip_serializing_none]
18#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
19pub struct CollectivePerceptionMessage {
20 pub protocol_version: u8,
21 pub station_id: u32,
22 pub generation_delta_time: u16,
24 pub management_container: ManagementContainer,
25 pub station_data_container: Option<StationDataContainer>,
26 #[serde(skip_serializing_if = "Vec::is_empty", default)]
27 pub sensor_information_container: Vec<SensorInformation>,
28 #[serde(skip_serializing_if = "Vec::is_empty", default)]
29 pub perceived_object_container: Vec<PerceivedObject>,
30 }
32
33impl CollectivePerceptionMessage {
34 pub fn mobile_perceived_object_list(&self) -> Vec<MobilePerceivedObject> {
35 if let Some(station_data_container) = &self.station_data_container {
36 if let Some(originating_vehicle_container) =
37 &station_data_container.originating_vehicle_container
38 {
39 return self
40 .perceived_object_container
41 .iter()
42 .map(|perceived_object| {
43 MobilePerceivedObject::new(
44 perceived_object.clone(),
47 self.station_id,
48 &self.management_container.reference_position,
49 originating_vehicle_container.heading,
50 )
51 })
52 .collect();
53 }
54 }
55 Vec::new()
56 }
57}
58
59#[serde_with::skip_serializing_none]
60#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
61pub struct ManagementContainer {
62 pub station_type: u8,
63 pub reference_position: ReferencePosition,
64 pub confidence: PositionConfidence,
65}
66
67#[serde_with::skip_serializing_none]
68#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
69pub struct StationDataContainer {
70 pub originating_vehicle_container: Option<OriginatingVehicleContainer>,
71 pub originating_rsu_container: Option<OriginatingRSUContainer>,
72}
73
74#[serde_with::skip_serializing_none]
75#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
76pub struct OriginatingVehicleContainer {
77 pub heading: u16,
78 pub speed: u16,
79 pub drive_direction: Option<u8>,
80 pub vehicle_length: Option<u16>,
81 pub vehicle_width: Option<u8>,
82 pub longitudinal_acceleration: Option<i16>,
83 pub yaw_rate: Option<i16>,
84 pub lateral_acceleration: Option<i16>,
85 pub vertical_acceleration: Option<i16>,
86 pub confidence: OriginatingVehicleContainerConfidence,
87}
88
89#[serde_with::skip_serializing_none]
90#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
91pub struct OriginatingRSUContainer {
92 pub intersection_reference_id: Option<IntersectionReferenceId>,
93 pub road_segment_reference_id: Option<u32>,
94}
95
96#[serde_with::skip_serializing_none]
97#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
98pub struct IntersectionReferenceId {
99 pub road_regulator_id: Option<u32>,
100 pub intersection_id: u32,
101}
102
103#[serde_with::skip_serializing_none]
104#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
105pub struct OriginatingVehicleContainerConfidence {
106 pub heading: u8,
107 pub speed: u8,
108 pub vehicle_length: Option<u8>,
109 pub yaw_rate: Option<u8>,
110 pub longitudinal_acceleration: Option<u8>,
111 pub lateral_acceleration: Option<u8>,
112 pub vertical_acceleration: Option<u8>,
113}
114
115#[serde_with::skip_serializing_none]
116#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
117pub struct SensorInformation {
118 pub sensor_id: u8,
119 #[serde(rename = "type")]
120 pub sensor_type: u8,
121 pub detection_area: DetectionArea,
122}
123
124#[serde_with::skip_serializing_none]
125#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
126pub struct DetectionArea {
127 pub vehicle_sensor: Option<VehicleSensor>,
128}
129
130#[serde_with::skip_serializing_none]
131#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
132pub struct VehicleSensor {
133 pub ref_point_id: u8,
134 pub x_sensor_offset: i16,
135 pub y_sensor_offset: i16,
136 pub z_sensor_offset: Option<u16>,
137 #[serde(skip_serializing_if = "Vec::is_empty", default)]
138 pub vehicle_sensor_property_list: Vec<VehicleSensorProperty>,
139}
140
141#[serde_with::skip_serializing_none]
142#[derive(Default, Debug, Clone, Hash, PartialEq, Serialize, Deserialize)]
143pub struct VehicleSensorProperty {
144 pub range: u16,
145 pub horizontal_opening_angle_start: u16,
146 pub horizontal_opening_angle_end: u16,
147 pub vertical_opening_angle_start: Option<u16>,
148 pub vertical_opening_angle_end: Option<u16>,
149}
150
151impl Mobile for CollectivePerceptionMessage {
152 fn mobile_id(&self) -> u32 {
153 self.station_id
154 }
155
156 fn position(&self) -> &ReferencePosition {
157 &self.management_container.reference_position
158 }
159
160 fn speed(&self) -> Option<u16> {
161 if let Some(station_data_container) = &self.station_data_container {
162 if let Some(originating_vehicle_container) =
163 &station_data_container.originating_vehicle_container
164 {
165 return Some(originating_vehicle_container.speed);
166 }
167 }
168 None
169 }
170
171 fn heading(&self) -> Option<u16> {
172 if let Some(station_data_container) = &self.station_data_container {
173 if let Some(originating_vehicle_container) =
174 &station_data_container.originating_vehicle_container
175 {
176 return Some(originating_vehicle_container.heading);
177 }
178 }
179 None
180 }
181}
182
183impl Typed for CollectivePerceptionMessage {
184 fn get_type() -> String {
185 "cpm".to_string()
186 }
187}
188
189#[cfg(test)]
190mod tests {
191 use crate::reception::exchange::collective_perception_message::{
192 CollectivePerceptionMessage, DetectionArea, ManagementContainer,
193 OriginatingVehicleContainer, SensorInformation, StationDataContainer, VehicleSensor,
194 VehicleSensorProperty,
195 };
196 use crate::reception::exchange::mobile_perceived_object::MobilePerceivedObject;
197 use crate::reception::exchange::perceived_object::{ObjectConfidence, PerceivedObject};
198 use crate::reception::exchange::reference_position::ReferencePosition;
199
200 fn create_perceived_object_in_front() -> PerceivedObject {
201 PerceivedObject {
202 object_id: 45,
203 time_of_measurement: 50,
204 confidence: ObjectConfidence {
205 x_distance: 102,
206 y_distance: 102,
207 x_speed: 127,
208 y_speed: 127,
209 object: Some(10),
210 },
211 x_distance: 40,
212 y_distance: 100,
213 x_speed: 1400,
214 y_speed: 500,
215 object_age: Default::default(),
216 object_ref_point: Some(0),
217 z_distance: None,
218 z_speed: None,
219 x_acceleration: None,
220 y_acceleration: None,
221 z_acceleration: None,
222 roll_angle: None,
223 pitch_angle: None,
224 yaw_angle: None,
225 roll_rate: None,
226 pitch_rate: None,
227 yaw_rate: None,
228 roll_acceleration: None,
229 pitch_acceleration: None,
230 yaw_acceleration: None,
231 lower_triangular_correlation_matrix_columns: vec![],
232 planar_object_dimension_1: None,
233 planar_object_dimension_2: None,
234 vertical_object_dimension: None,
235 sensor_id_list: vec![],
236 dynamic_status: None,
237 classification: vec![],
238 matched_position: None,
239 }
240 }
241
242 fn create_perceived_object_behind() -> PerceivedObject {
243 PerceivedObject {
244 object_id: 48,
245 time_of_measurement: 51,
246 confidence: ObjectConfidence {
247 x_distance: 102,
248 y_distance: 102,
249 x_speed: 127,
250 y_speed: 127,
251 object: Some(10),
252 },
253 x_distance: -40,
254 y_distance: 100,
255 x_speed: 1200,
256 y_speed: 400,
257 object_age: Default::default(),
258 object_ref_point: Some(0),
259 z_distance: None,
260 z_speed: None,
261 x_acceleration: None,
262 y_acceleration: None,
263 z_acceleration: None,
264 roll_angle: None,
265 pitch_angle: None,
266 yaw_angle: None,
267 roll_rate: None,
268 pitch_rate: None,
269 yaw_rate: None,
270 roll_acceleration: None,
271 pitch_acceleration: None,
272 yaw_acceleration: None,
273 lower_triangular_correlation_matrix_columns: vec![],
274 planar_object_dimension_1: None,
275 planar_object_dimension_2: None,
276 vertical_object_dimension: None,
277 sensor_id_list: vec![],
278 dynamic_status: None,
279 classification: vec![],
280 matched_position: None,
281 }
282 }
283
284 fn create_cpm_with_two_perceived_object() -> CollectivePerceptionMessage {
285 CollectivePerceptionMessage {
286 protocol_version: 1,
287 station_id: 31470,
288 generation_delta_time: 65535,
290 management_container: ManagementContainer {
291 station_type: 5,
292 reference_position: ReferencePosition {
293 latitude: 426263556,
294 longitude: -82492123,
295 altitude: 800001,
296 },
297 confidence: Default::default(),
298 },
299 station_data_container: Some(StationDataContainer {
300 originating_vehicle_container: Some(OriginatingVehicleContainer {
301 heading: 900,
302 speed: 1600,
303 ..Default::default()
304 }),
305 ..Default::default()
306 }),
307 sensor_information_container: vec![SensorInformation {
308 sensor_id: 3,
309 sensor_type: 3,
310 detection_area: DetectionArea {
311 vehicle_sensor: Some(VehicleSensor {
312 ref_point_id: 0,
313 x_sensor_offset: -20,
314 y_sensor_offset: 20,
315 z_sensor_offset: Some(0),
316 vehicle_sensor_property_list: vec![VehicleSensorProperty {
317 range: 5000,
318 horizontal_opening_angle_start: 600,
319 horizontal_opening_angle_end: 600,
320 vertical_opening_angle_start: None,
321 vertical_opening_angle_end: None,
322 }],
323 }),
324 },
325 }],
326 perceived_object_container: vec![
327 create_perceived_object_in_front(),
328 create_perceived_object_behind(),
329 ],
330 }
331 }
332
333 #[test]
334 fn it_can_provide_the_mobile_perceived_object_list() {
335 assert_eq!(
336 create_cpm_with_two_perceived_object().mobile_perceived_object_list(),
337 vec![
338 MobilePerceivedObject {
339 perceived_object: create_perceived_object_in_front(),
340 mobile_id: 3147045,
341 reference_position: ReferencePosition {
342 latitude: 426263645,
343 longitude: -82492074,
344 altitude: 800001,
345 },
346 speed: 1486,
347 heading: 900
348 },
349 MobilePerceivedObject {
350 perceived_object: create_perceived_object_behind(),
351 mobile_id: 3147048,
352 reference_position: ReferencePosition {
353 latitude: 426263645,
354 longitude: -82492170,
355 altitude: 800001,
356 },
357 speed: 1264,
358 heading: 900
359 },
360 ]
361 );
362 }
363
364 #[test]
365 fn test_deserialize() {
366 let data = r#"{
367 "protocol_version": 0,
368 "station_id": 0,
369 "generation_delta_time": 0,
370 "management_container": {
371 "station_type": 15,
372 "reference_position": {
373 "latitude": 488640493,
374 "longitude": 23310526,
375 "altitude": 900
376 },
377 "confidence": {
378 "position_confidence_ellipse": {
379 "semi_major_confidence": 1,
380 "semi_minor_confidence": 1,
381 "semi_major_orientation": 0
382 },
383 "altitude": 0
384 }
385 },
386 "perceived_object_container": [
387 {
388 "object_id": 5,
389 "time_of_measurement": 2,
390 "x_distance": 804,
391 "y_distance": 400,
392 "x_speed": 401,
393 "y_speed": 401,
394 "object_age": 1500,
395 "dynamic_status": 0,
396 "classification": [
397 {
398 "object_class": {
399 "single_vru": {
400 "pedestrian": 1
401 }
402 },
403 "confidence": 40
404 }
405 ],
406 "confidence": {
407 "x_distance": 4095,
408 "y_distance": 4095,
409 "x_speed": 0,
410 "y_speed": 0,
411 "object": 10
412 }
413 },
414 {
415 "object_id": 7,
416 "time_of_measurement": 2,
417 "x_distance": -1594,
418 "y_distance": 540,
419 "x_speed": 652,
420 "y_speed": 652,
421 "object_age": 1500,
422 "dynamic_status": 0,
423 "classification": [
424 {
425 "object_class": {
426 "single_vru": {
427 "pedestrian": 1
428 }
429 },
430 "confidence": 40
431 }
432 ],
433 "confidence": {
434 "x_distance": 4095,
435 "y_distance": 4095,
436 "x_speed": 0,
437 "y_speed": 0,
438 "object": 10
439 }
440 },
441 {
442 "object_id": 9,
443 "time_of_measurement": 3,
444 "x_distance": 1009,
445 "y_distance": 581,
446 "x_speed": 283,
447 "y_speed": 283,
448 "object_age": 1500,
449 "dynamic_status": 0,
450 "classification": [
451 {
452 "object_class": {
453 "single_vru": {
454 "pedestrian": 1
455 }
456 },
457 "confidence": 40
458 }
459 ],
460 "confidence": {
461 "x_distance": 4095,
462 "y_distance": 4095,
463 "x_speed": 0,
464 "y_speed": 0,
465 "object": 10
466 }
467 },
468 {
469 "object_id": 11,
470 "time_of_measurement": 3,
471 "x_distance": -224,
472 "y_distance": 3077,
473 "x_speed": 343,
474 "y_speed": 343,
475 "object_age": 1500,
476 "dynamic_status": 0,
477 "classification": [
478 {
479 "object_class": {
480 "single_vru": {
481 "pedestrian": 1
482 }
483 },
484 "confidence": 40
485 }
486 ],
487 "confidence": {
488 "x_distance": 4095,
489 "y_distance": 4095,
490 "x_speed": 0,
491 "y_speed": 0,
492 "object": 10
493 }
494 },
495 {
496 "object_id": 12,
497 "time_of_measurement": 4,
498 "x_distance": 3329,
499 "y_distance": -813,
500 "x_speed": 735,
501 "y_speed": 735,
502 "object_age": 1500,
503 "dynamic_status": 0,
504 "classification": [
505 {
506 "object_class": {
507 "single_vru": {
508 "pedestrian": 1
509 }
510 },
511 "confidence": 40
512 }
513 ],
514 "confidence": {
515 "x_distance": 4095,
516 "y_distance": 4095,
517 "x_speed": 0,
518 "y_speed": 0,
519 "object": 10
520 }
521 },
522 {
523 "object_id": 88,
524 "time_of_measurement": 7,
525 "x_distance": 1056,
526 "y_distance": 979,
527 "y_speed": 7,
528 "x_speed": 7,
529 "object_age": 1500,
530 "dynamic_status": 0,
531 "classification": [
532 {
533 "object_class": {
534 "single_vru": {
535 "pedestrian": 1
536 }
537 },
538 "confidence": 40
539 }
540 ],
541 "confidence": {
542 "x_distance": 4095,
543 "y_distance": 4095,
544 "x_speed": 0,
545 "y_speed": 0,
546 "object": 10
547 }
548 },
549 {
550 "object_id": 195,
551 "time_of_measurement": 7,
552 "x_distance": -365,
553 "y_distance": 2896,
554 "x_speed": 514,
555 "y_speed": 514,
556 "object_age": 1500,
557 "dynamic_status": 0,
558 "classification": [
559 {
560 "object_class": {
561 "single_vru": {
562 "pedestrian": 1
563 }
564 },
565 "confidence": 40
566 }
567 ],
568 "confidence": {
569 "x_distance": 4095,
570 "y_distance": 4095,
571 "x_speed": 0,
572 "y_speed": 0,
573 "object": 10
574 }
575 },
576 {
577 "object_id": 200,
578 "time_of_measurement": 7,
579 "x_distance": 42,
580 "y_distance": 1523,
581 "x_speed": 948,
582 "y_speed": 948,
583 "object_age": 1500,
584 "dynamic_status": 0,
585 "classification": [
586 {
587 "object_class": {
588 "single_vru": {
589 "bicyclist": 1
590 }
591 },
592 "confidence": 36
593 }
594 ],
595 "confidence": {
596 "x_distance": 4095,
597 "y_distance": 4095,
598 "x_speed": 0,
599 "y_speed": 0,
600 "object": 10
601 }
602 },
603 {
604 "object_id": 206,
605 "time_of_measurement": 7,
606 "x_distance": -857,
607 "y_distance": 117,
608 "x_speed": 241,
609 "y_speed": 241,
610 "object_age": 1500,
611 "dynamic_status": 0,
612 "classification": [
613 {
614 "object_class": {
615 "single_vru": {
616 "bicyclist": 1
617 }
618 },
619 "confidence": 36
620 }
621 ],
622 "confidence": {
623 "x_distance": 4095,
624 "y_distance": 4095,
625 "x_speed": 0,
626 "y_speed": 0,
627 "object": 10
628 }
629 },
630 {
631 "object_id": 214,
632 "time_of_measurement": 8,
633 "x_distance": 776,
634 "y_distance": 498,
635 "x_speed": 223,
636 "y_speed": 223,
637 "object_age": 1500,
638 "dynamic_status": 0,
639 "classification": [
640 {
641 "object_class": {
642 "single_vru": {
643 "pedestrian": 1
644 }
645 },
646 "confidence": 40
647 }
648 ],
649 "confidence": {
650 "x_distance": 4095,
651 "y_distance": 4095,
652 "x_speed": 0,
653 "y_speed": 0,
654 "object": 10
655 }
656 },
657 {
658 "object_id": 225,
659 "time_of_measurement": 9,
660 "x_distance": -103,
661 "y_distance": 511,
662 "x_speed": 556,
663 "y_speed": 556,
664 "object_age": 1500,
665 "dynamic_status": 0,
666 "classification": [
667 {
668 "object_class": {
669 "single_vru": {
670 "bicyclist": 1
671 }
672 },
673 "confidence": 39
674 }
675 ],
676 "confidence": {
677 "x_distance": 4095,
678 "y_distance": 4095,
679 "x_speed": 0,
680 "y_speed": 0,
681 "object": 10
682 }
683 },
684 {
685 "object_id": 229,
686 "time_of_measurement": 9,
687 "x_distance": 1603,
688 "y_distance": 517,
689 "x_speed": 154,
690 "y_speed": 154,
691 "object_age": 1500,
692 "dynamic_status": 0,
693 "classification": [
694 {
695 "object_class": {
696 "single_vru": {
697 "pedestrian": 1
698 }
699 },
700 "confidence": 46
701 }
702 ],
703 "confidence": {
704 "x_distance": 4095,
705 "y_distance": 4095,
706 "x_speed": 0,
707 "y_speed": 0,
708 "object": 10
709 }
710 },
711 {
712 "object_id": 241,
713 "time_of_measurement": 10,
714 "x_distance": 1872,
715 "y_distance": -164,
716 "x_speed": 134,
717 "y_speed": 134,
718 "object_age": 1500,
719 "dynamic_status": 0,
720 "classification": [
721 {
722 "object_class": {
723 "single_vru": {
724 "pedestrian": 1
725 }
726 },
727 "confidence": 44
728 }
729 ],
730 "confidence": {
731 "x_distance": 4095,
732 "y_distance": 4095,
733 "x_speed": 0,
734 "y_speed": 0,
735 "object": 10
736 }
737 },
738 {
739 "object_id": 244,
740 "time_of_measurement": 10,
741 "x_distance": 611,
742 "y_distance": 339,
743 "x_speed": 283,
744 "y_speed": 283,
745 "object_age": 1500,
746 "dynamic_status": 0,
747 "classification": [
748 {
749 "object_class": {
750 "single_vru": {
751 "pedestrian": 1
752 }
753 },
754 "confidence": 44
755 }
756 ],
757 "confidence": {
758 "x_distance": 4095,
759 "y_distance": 4095,
760 "x_speed": 0,
761 "y_speed": 0,
762 "object": 10
763 }
764 },
765 {
766 "object_id": 247,
767 "time_of_measurement": 10,
768 "x_distance": -645,
769 "y_distance": 12,
770 "x_speed": 735,
771 "y_speed": 735,
772 "object_age": 1500,
773 "dynamic_status": 0,
774 "classification": [
775 {
776 "object_class": {
777 "single_vru": {
778 "pedestrian": 1
779 }
780 },
781 "confidence": 44
782 }
783 ],
784 "confidence": {
785 "x_distance": 4095,
786 "y_distance": 4095,
787 "x_speed": 0,
788 "y_speed": 0,
789 "object": 10
790 }
791 },
792 {
793 "object_id": 249,
794 "time_of_measurement": 10,
795 "x_distance": 4360,
796 "y_distance": -262,
797 "x_speed": 513,
798 "y_speed": 513,
799 "object_age": 1500,
800 "dynamic_status": 0,
801 "classification": [
802 {
803 "object_class": {
804 "single_vru": {
805 "pedestrian": 1
806 }
807 },
808 "confidence": 44
809 }
810 ],
811 "confidence": {
812 "x_distance": 4095,
813 "y_distance": 4095,
814 "x_speed": 0,
815 "y_speed": 0,
816 "object": 10
817 }
818 }
819 ]
820 }"#;
821
822 match serde_json::from_str::<CollectivePerceptionMessage>(data) {
823 Ok(cpm) => {
824 assert_eq!(cpm.station_id, 0);
825 }
826 Err(e) => {
827 panic!("Failed to deserialize CPM: '{}'", e);
828 }
829 }
830 }
831
832 #[test]
833 fn test_deserialize_full_cpm() {
834 let data = r#"{
835 "protocol_version": 255,
836 "station_id": 4294967295,
837 "generation_delta_time": 65535,
838 "management_container": {
839 "station_type": 254,
840 "reference_position": {
841 "latitude": 426263556,
842 "longitude": -82492123,
843 "altitude": 800001
844 },
845 "confidence": {
846 "position_confidence_ellipse": {
847 "semi_major_confidence": 4095,
848 "semi_minor_confidence": 4095,
849 "semi_major_orientation": 3601
850 },
851 "altitude": 15
852 }
853 },
854 "station_data_container": {
855 "originating_vehicle_container": {
856 "heading": 180,
857 "speed": 1600,
858 "drive_direction": 0,
859 "vehicle_length": 31,
860 "vehicle_width": 18,
861 "longitudinal_acceleration": -160,
862 "yaw_rate": -32766,
863 "lateral_acceleration": -2,
864 "vertical_acceleration": -1,
865 "confidence": {
866 "heading": 127,
867 "speed": 127,
868 "vehicle_length": 3,
869 "yaw_rate": 2,
870 "longitudinal_acceleration": 12,
871 "lateral_acceleration": 13,
872 "vertical_acceleration": 14
873 }
874 }
875 },
876 "sensor_information_container": [{
877 "sensor_id": 1,
878 "type": 3,
879 "detection_area": {
880 "vehicle_sensor": {
881 "ref_point_id": 255,
882 "x_sensor_offset": -3094,
883 "y_sensor_offset": -1000,
884 "z_sensor_offset": 1000,
885 "vehicle_sensor_property_list": [{
886 "range": 10000,
887 "horizontal_opening_angle_start": 3601,
888 "horizontal_opening_angle_end": 3601,
889 "vertical_opening_angle_start": 3601,
890 "vertical_opening_angle_end": 3601
891 }]
892 },
893 "stationary_sensor_radial": {
894 "range": 10000,
895 "horizontal_opening_angle_start": 3601,
896 "horizontal_opening_angle_end": 3601,
897 "vertical_opening_angle_start": 3601,
898 "vertical_opening_angle_end": 3601,
899 "sensor_position_offset": {
900 "x": 32767,
901 "y": -32768,
902 "z": 0
903 }
904 },
905 "stationary_sensor_polygon": [{
906 "x": -32768,
907 "y": 32767,
908 "z": 0
909 },
910 {
911 "x": 10,
912 "y": 20,
913 "z": 30
914 },
915 {
916 "x": 32767,
917 "y": -32768,
918 "z": 0
919 }
920 ],
921 "stationary_sensor_circular": {
922 "radius": 10000,
923 "node_center_point": {
924 "x": 32767,
925 "y": -32768,
926 "z": 0
927 }
928 },
929 "stationary_sensor_ellipse": {
930 "node_center_point": {
931 "x": 32767,
932 "y": -32768,
933 "z": 0
934 },
935 "semi_major_range_length": 10000,
936 "semi_minor_range_length": 10000,
937 "semi_major_range_orientation": 3601,
938 "semi_height": 10000
939 },
940 "stationary_sensor_rectangle": {
941 "node_center_point": {
942 "x": 32767,
943 "y": -32768,
944 "z": 0
945 },
946 "semi_major_range_length": 10000,
947 "semi_minor_range_length": 10000,
948 "semi_major_range_orientation": 3601,
949 "semi_height": 10000
950 }
951 }
952 }],
953 "perceived_object_container": [{
954 "object_id": 0,
955 "time_of_measurement": 50,
956 "x_distance": 400,
957 "y_distance": 100,
958 "z_distance": 50,
959 "x_speed": 1400,
960 "y_speed": 500,
961 "z_speed": 0,
962 "x_acceleration": -160,
963 "y_acceleration": 0,
964 "z_acceleration": 161,
965 "roll_angle": 0,
966 "pitch_angle": 3600,
967 "yaw_angle": 3601,
968 "roll_rate": -32766,
969 "pitch_rate": 0,
970 "yaw_rate": 32767,
971 "roll_acceleration": -32766,
972 "pitch_acceleration": 0,
973 "yaw_acceleration": 32767,
974 "lower_triangular_correlation_matrix_columns": [
975 [-100, -99, -98],
976 [0, 1, 2],
977 [98, 99, 100]
978 ],
979 "planar_object_dimension_1": 1023,
980 "planar_object_dimension_2": 1023,
981 "vertical_object_dimension": 1023,
982 "object_ref_point": 8,
983 "confidence": {
984 "x_distance": 102,
985 "y_distance": 102,
986 "z_distance": 102,
987 "x_speed": 7,
988 "y_speed": 7,
989 "z_speed": 7,
990 "x_acceleration": 102,
991 "y_acceleration": 102,
992 "z_acceleration": 102,
993 "roll_angle": 127,
994 "pitch_angle": 127,
995 "yaw_angle": 127,
996 "roll_rate": 8,
997 "pitch_rate": 8,
998 "yaw_rate": 8,
999 "roll_acceleration": 8,
1000 "pitch_acceleration": 8,
1001 "yaw_acceleration": 8,
1002 "planar_object_dimension_1": 102,
1003 "planar_object_dimension_2": 102,
1004 "vertical_object_dimension": 102,
1005 "longitudinal_lane_position": 102,
1006 "object": 10
1007 },
1008 "object_age": 1500,
1009 "sensor_id_list": [1, 2, 10, 100, 255],
1010 "dynamic_status": 2,
1011 "classification": [{
1012 "object_class": {
1013 "vehicle": 10
1014 },
1015 "confidence": 101
1016 },
1017 {
1018 "object_class": {
1019 "single_vru": {
1020 "pedestrian": 2
1021 }
1022 },
1023 "confidence": 25
1024 },
1025 {
1026 "object_class": {
1027 "vru_group": {
1028 "group_type": {
1029 "pedestrian": true,
1030 "bicyclist": false,
1031 "motorcyclist": false,
1032 "animal": true
1033 },
1034 "group_size": 12,
1035 "cluster_id": 255
1036 }
1037 },
1038 "confidence": 64
1039 },
1040 {
1041 "object_class": {
1042 "other": 1
1043 },
1044 "confidence": 0
1045 }
1046 ],
1047 "matched_position": {
1048 "lane_id": 255,
1049 "longitudinal_lane_position": 32767
1050 }
1051 }],
1052 "free_space_addendum_container": [{
1053 "free_space_area": {
1054 "free_space_polygon": [{
1055 "x": -32768,
1056 "y": 32767,
1057 "z": 0
1058 },
1059 {
1060 "x": 10,
1061 "y": 20,
1062 "z": 30
1063 },
1064 {
1065 "x": 32767,
1066 "y": -32768,
1067 "z": 0
1068 }
1069 ]
1070 },
1071 "free_space_confidence": 0,
1072 "sensor_id_list": [0, 1, 2],
1073 "shadowing_applies": false
1074 },
1075 {
1076 "free_space_area": {
1077 "free_space_circular": {
1078 "radius": 10000,
1079 "node_center_point": {
1080 "x": 32767,
1081 "y": -32768,
1082 "z": 0
1083 }
1084 }
1085 },
1086 "free_space_confidence": 101,
1087 "sensor_id_list": [10, 20, 30],
1088 "shadowing_applies": true
1089 },
1090 {
1091 "free_space_area": {
1092 "free_space_ellipse": {
1093 "node_center_point": {
1094 "x": 32767,
1095 "y": -32768,
1096 "z": 0
1097 },
1098 "semi_major_range_length": 10000,
1099 "semi_minor_range_length": 10000,
1100 "semi_major_range_orientation": 3601,
1101 "semi_height": 10000
1102 }
1103 },
1104 "free_space_confidence": 0,
1105 "sensor_id_list": [100, 102, 103],
1106 "shadowing_applies": false
1107 },
1108 {
1109 "free_space_area": {
1110 "free_space_rectangle": {
1111 "node_center_point": {
1112 "x": 32767,
1113 "y": -32768,
1114 "z": 0
1115 },
1116 "semi_major_range_length": 10000,
1117 "semi_minor_range_length": 10000,
1118 "semi_major_range_orientation": 3601,
1119 "semi_height": 10000
1120 }
1121 },
1122 "free_space_confidence": 50,
1123 "sensor_id_list": [200, 250, 255],
1124 "shadowing_applies": true
1125 }
1126 ]
1127 }"#;
1128
1129 match serde_json::from_str::<CollectivePerceptionMessage>(data) {
1130 Ok(cpm) => {
1131 assert_eq!(cpm.station_id, 4294967295);
1132 }
1133 Err(e) => {
1134 panic!("Failed to deserialize CPM: '{}'", e);
1135 }
1136 }
1137 }
1138}