edgefirst_schemas/
lib.rs

1// SPDX-License-Identifier: Apache-2.0
2// Copyright © 2025 Au-Zone Technologies. All Rights Reserved.
3
4//! # EdgeFirst Middleware Schemas
5//!
6//! This library provides the Rust structs for EdgeFirst Middleware messages.
7//!
8//! Common Rust struct for ROS 2 messages used by EdgeFirst Middleware Services with Zenoh.
9//!
10//! Here are some ROS message source:
11//!
12//! * [common_interface](https://github.com/ros2/common_interfaces): Common-used ROS message
13//! * [rcl_interface](https://github.com/ros2/rcl_interfaces): Common interface in RCL
14//! * [foxglove_api_msgs](https://github.com/foxglove/schemas/tree/main/ros_foxglove_msgs)
15//! * [edgefirst_api_msgs](https://github.com/EdgeFirstAI/schemas): EdgeFirst ROS messages
16
17/// EdgeFirst Messages
18pub mod edgefirst_msgs;
19
20/// Foxglove Messages
21pub mod foxglove_msgs;
22
23/// ROS 2 Common Interfaces
24pub mod geometry_msgs;
25pub mod sensor_msgs;
26pub mod std_msgs;
27
28/// ROS 2 RCL Interfaces
29pub mod builtin_interfaces;
30pub mod rosgraph_msgs;
31
32pub mod service;
33
34/// CDR serialization/deserialization support
35pub mod serde_cdr;
36
37/// Schema registry for runtime schema lookup
38pub mod schema_registry;
39
40/// C FFI bindings
41mod ffi;
42
43use sensor_msgs::{point_field, PointCloud2, PointField};
44use std::collections::HashMap;
45
46const SIZE_OF_DATATYPE: [usize; 9] = [
47    0, 1, // pub const INT8: u8 = 1;
48    1, // pub const UINT8: u8 = 2;
49    2, // pub const INT16: u8 = 3;
50    2, // pub const UINT16: u8 = 4;
51    4, // pub const INT32: u8 = 5;
52    4, // pub const UINT32: u8 = 6;
53    4, // pub const FLOAT32: u8 = 7;
54    8, //pub const FLOAT64: u8 = 8;
55];
56
57pub struct Point {
58    pub x: f64,
59    pub y: f64,
60    pub z: f64,
61    pub id: isize,
62    pub fields: HashMap<String, f64>,
63}
64
65/// This function takes a PointCloud2 message and decodes it into a vector of Points.
66/// Each Point contains the x, y, z coordinates, an id, and a HashMap of additional fields.
67pub fn decode_pcd(pcd: &PointCloud2) -> Vec<Point> {
68    let mut points = Vec::new();
69    for i in 0..pcd.height {
70        for j in 0..pcd.width {
71            let start = (i * pcd.row_step + j * pcd.point_step) as usize;
72            let end = start + pcd.point_step as usize;
73            let p = if pcd.is_bigendian {
74                parse_point_be(&pcd.fields, &pcd.data[start..end])
75            } else {
76                parse_point_le(&pcd.fields, &pcd.data[start..end])
77            };
78            points.push(p);
79        }
80    }
81    points
82}
83
84fn parse_point_le(fields: &[PointField], data: &[u8]) -> Point {
85    let mut p = Point {
86        x: 0.0,
87        y: 0.0,
88        z: 0.0,
89        id: 0,
90        fields: HashMap::new(),
91    };
92    for f in fields {
93        let start = f.offset as usize;
94        let val = match f.datatype {
95            point_field::INT8 => {
96                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::INT8 as usize]]
97                    .try_into()
98                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
99                i8::from_le_bytes(bytes) as f64
100            }
101            point_field::UINT8 => {
102                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::UINT8 as usize]]
103                    .try_into()
104                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
105                u8::from_le_bytes(bytes) as f64
106            }
107            point_field::INT16 => {
108                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::INT16 as usize]]
109                    .try_into()
110                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
111                i16::from_le_bytes(bytes) as f64
112            }
113            point_field::UINT16 => {
114                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::UINT16 as usize]]
115                    .try_into()
116                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
117                u16::from_le_bytes(bytes) as f64
118            }
119            point_field::INT32 => {
120                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::INT32 as usize]]
121                    .try_into()
122                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
123                i32::from_le_bytes(bytes) as f64
124            }
125            point_field::UINT32 => {
126                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::UINT32 as usize]]
127                    .try_into()
128                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
129                u32::from_le_bytes(bytes) as f64
130            }
131            point_field::FLOAT32 => {
132                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::FLOAT32 as usize]]
133                    .try_into()
134                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
135                f32::from_le_bytes(bytes) as f64
136            }
137            point_field::FLOAT64 => {
138                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::FLOAT64 as usize]]
139                    .try_into()
140                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
141                f64::from_le_bytes(bytes)
142            }
143            _ => {
144                // Unknown datatype in PointField
145                continue;
146            }
147        };
148        match f.name.as_str() {
149            "x" => p.x = val,
150            "y" => p.y = val,
151            "z" => p.z = val,
152            "cluster_id" => p.id = val as isize,
153            _ => {
154                p.fields.insert(f.name.clone(), val);
155            }
156        }
157    }
158    p
159}
160
161fn parse_point_be(fields: &[PointField], data: &[u8]) -> Point {
162    let mut p = Point {
163        x: 0.0,
164        y: 0.0,
165        z: 0.0,
166        id: 0,
167        fields: HashMap::new(),
168    };
169    for f in fields {
170        let start = f.offset as usize;
171
172        let val = match f.datatype {
173            point_field::INT8 => {
174                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::INT8 as usize]]
175                    .try_into()
176                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
177                i8::from_be_bytes(bytes) as f64
178            }
179            point_field::UINT8 => {
180                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::UINT8 as usize]]
181                    .try_into()
182                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
183                u8::from_be_bytes(bytes) as f64
184            }
185            point_field::INT16 => {
186                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::INT16 as usize]]
187                    .try_into()
188                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
189                i16::from_be_bytes(bytes) as f64
190            }
191            point_field::UINT16 => {
192                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::UINT16 as usize]]
193                    .try_into()
194                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
195                u16::from_be_bytes(bytes) as f64
196            }
197            point_field::INT32 => {
198                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::INT32 as usize]]
199                    .try_into()
200                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
201                i32::from_be_bytes(bytes) as f64
202            }
203            point_field::UINT32 => {
204                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::UINT32 as usize]]
205                    .try_into()
206                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
207                u32::from_be_bytes(bytes) as f64
208            }
209            point_field::FLOAT32 => {
210                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::FLOAT32 as usize]]
211                    .try_into()
212                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
213                f32::from_be_bytes(bytes) as f64
214            }
215            point_field::FLOAT64 => {
216                let bytes = data[start..start + SIZE_OF_DATATYPE[point_field::FLOAT64 as usize]]
217                    .try_into()
218                    .unwrap_or_else(|e| panic!("Expected slice with 1 element: {:?}", e));
219                f64::from_be_bytes(bytes)
220            }
221            _ => {
222                // "Unknown datatype in PointField
223                continue;
224            }
225        };
226        match f.name.as_str() {
227            "x" => p.x = val,
228            "y" => p.y = val,
229            "z" => p.z = val,
230            _ => {
231                p.fields.insert(f.name.clone(), val);
232            }
233        }
234    }
235
236    p
237}
238
239#[cfg(test)]
240mod tests {
241    use super::*;
242    use crate::builtin_interfaces::Time;
243    use crate::std_msgs::Header;
244
245    /// Helper to create a PointCloud2 with FLOAT32 x/y/z fields
246    fn make_xyz_cloud(points_data: &[[f32; 3]], is_bigendian: bool) -> PointCloud2 {
247        let fields = vec![
248            PointField {
249                name: "x".to_string(),
250                offset: 0,
251                datatype: point_field::FLOAT32,
252                count: 1,
253            },
254            PointField {
255                name: "y".to_string(),
256                offset: 4,
257                datatype: point_field::FLOAT32,
258                count: 1,
259            },
260            PointField {
261                name: "z".to_string(),
262                offset: 8,
263                datatype: point_field::FLOAT32,
264                count: 1,
265            },
266        ];
267
268        let point_step = 12u32;
269        let width = points_data.len() as u32;
270        let row_step = point_step * width;
271
272        let mut data = Vec::with_capacity(points_data.len() * 12);
273        for p in points_data {
274            for val in p {
275                if is_bigendian {
276                    data.extend_from_slice(&val.to_be_bytes());
277                } else {
278                    data.extend_from_slice(&val.to_le_bytes());
279                }
280            }
281        }
282
283        PointCloud2 {
284            header: Header {
285                stamp: Time::new(0, 0),
286                frame_id: "test".to_string(),
287            },
288            height: 1,
289            width,
290            fields,
291            is_bigendian,
292            point_step,
293            row_step,
294            data,
295            is_dense: true,
296        }
297    }
298
299    #[test]
300    fn decode_pcd_basic_xyz_little_endian() {
301        let input = [[1.0f32, 2.0, 3.0], [4.0, 5.0, 6.0], [-1.0, -2.0, -3.0]];
302        let cloud = make_xyz_cloud(&input, false);
303        let points = decode_pcd(&cloud);
304
305        assert_eq!(points.len(), 3);
306        assert!((points[0].x - 1.0).abs() < 1e-6);
307        assert!((points[0].y - 2.0).abs() < 1e-6);
308        assert!((points[0].z - 3.0).abs() < 1e-6);
309        assert!((points[1].x - 4.0).abs() < 1e-6);
310        assert!((points[2].x - (-1.0)).abs() < 1e-6);
311    }
312
313    #[test]
314    fn decode_pcd_basic_xyz_big_endian() {
315        let input = [[10.0f32, 20.0, 30.0]];
316        let cloud = make_xyz_cloud(&input, true);
317        let points = decode_pcd(&cloud);
318
319        assert_eq!(points.len(), 1);
320        assert!((points[0].x - 10.0).abs() < 1e-6);
321        assert!((points[0].y - 20.0).abs() < 1e-6);
322        assert!((points[0].z - 30.0).abs() < 1e-6);
323    }
324
325    #[test]
326    fn decode_pcd_empty_cloud() {
327        let cloud = PointCloud2 {
328            header: Header {
329                stamp: Time::new(0, 0),
330                frame_id: String::new(),
331            },
332            height: 0,
333            width: 0,
334            fields: vec![],
335            is_bigendian: false,
336            point_step: 0,
337            row_step: 0,
338            data: vec![],
339            is_dense: true,
340        };
341        let points = decode_pcd(&cloud);
342        assert!(points.is_empty());
343    }
344
345    #[test]
346    fn decode_pcd_with_cluster_id() {
347        // x(f32) + y(f32) + z(f32) + cluster_id(i32) = 16 bytes per point
348        let fields = vec![
349            PointField {
350                name: "x".to_string(),
351                offset: 0,
352                datatype: point_field::FLOAT32,
353                count: 1,
354            },
355            PointField {
356                name: "y".to_string(),
357                offset: 4,
358                datatype: point_field::FLOAT32,
359                count: 1,
360            },
361            PointField {
362                name: "z".to_string(),
363                offset: 8,
364                datatype: point_field::FLOAT32,
365                count: 1,
366            },
367            PointField {
368                name: "cluster_id".to_string(),
369                offset: 12,
370                datatype: point_field::INT32,
371                count: 1,
372            },
373        ];
374
375        let mut data = Vec::new();
376        // Point 1: (1,2,3) cluster_id=42
377        data.extend_from_slice(&1.0f32.to_le_bytes());
378        data.extend_from_slice(&2.0f32.to_le_bytes());
379        data.extend_from_slice(&3.0f32.to_le_bytes());
380        data.extend_from_slice(&42i32.to_le_bytes());
381        // Point 2: (4,5,6) cluster_id=-1
382        data.extend_from_slice(&4.0f32.to_le_bytes());
383        data.extend_from_slice(&5.0f32.to_le_bytes());
384        data.extend_from_slice(&6.0f32.to_le_bytes());
385        data.extend_from_slice(&(-1i32).to_le_bytes());
386
387        let cloud = PointCloud2 {
388            header: Header {
389                stamp: Time::new(100, 0),
390                frame_id: "lidar".to_string(),
391            },
392            height: 1,
393            width: 2,
394            fields,
395            is_bigendian: false,
396            point_step: 16,
397            row_step: 32,
398            data,
399            is_dense: true,
400        };
401
402        let points = decode_pcd(&cloud);
403        assert_eq!(points.len(), 2);
404        assert_eq!(points[0].id, 42);
405        assert_eq!(points[1].id, -1);
406    }
407
408    #[test]
409    fn decode_pcd_with_custom_fields() {
410        // Fusion output with vision_class field (as used in samples)
411        let fields = vec![
412            PointField {
413                name: "x".to_string(),
414                offset: 0,
415                datatype: point_field::FLOAT32,
416                count: 1,
417            },
418            PointField {
419                name: "y".to_string(),
420                offset: 4,
421                datatype: point_field::FLOAT32,
422                count: 1,
423            },
424            PointField {
425                name: "z".to_string(),
426                offset: 8,
427                datatype: point_field::FLOAT32,
428                count: 1,
429            },
430            PointField {
431                name: "vision_class".to_string(),
432                offset: 12,
433                datatype: point_field::FLOAT32,
434                count: 1,
435            },
436            PointField {
437                name: "intensity".to_string(),
438                offset: 16,
439                datatype: point_field::UINT8,
440                count: 1,
441            },
442        ];
443
444        let mut data = Vec::new();
445        data.extend_from_slice(&1.0f32.to_le_bytes());
446        data.extend_from_slice(&2.0f32.to_le_bytes());
447        data.extend_from_slice(&3.0f32.to_le_bytes());
448        data.extend_from_slice(&5.0f32.to_le_bytes()); // vision_class = 5
449        data.push(200u8); // intensity = 200
450                          // Pad to point_step
451        data.extend_from_slice(&[0u8; 3]);
452
453        let cloud = PointCloud2 {
454            header: Header {
455                stamp: Time::new(0, 0),
456                frame_id: "fusion".to_string(),
457            },
458            height: 1,
459            width: 1,
460            fields,
461            is_bigendian: false,
462            point_step: 20,
463            row_step: 20,
464            data,
465            is_dense: true,
466        };
467
468        let points = decode_pcd(&cloud);
469        assert_eq!(points.len(), 1);
470        assert!((points[0].fields["vision_class"] - 5.0).abs() < 1e-6);
471        assert!((points[0].fields["intensity"] - 200.0).abs() < 1e-6);
472    }
473
474    #[test]
475    fn decode_pcd_all_datatypes_little_endian() {
476        // Test all supported datatypes
477        let fields = vec![
478            PointField {
479                name: "i8".to_string(),
480                offset: 0,
481                datatype: point_field::INT8,
482                count: 1,
483            },
484            PointField {
485                name: "u8".to_string(),
486                offset: 1,
487                datatype: point_field::UINT8,
488                count: 1,
489            },
490            PointField {
491                name: "i16".to_string(),
492                offset: 2,
493                datatype: point_field::INT16,
494                count: 1,
495            },
496            PointField {
497                name: "u16".to_string(),
498                offset: 4,
499                datatype: point_field::UINT16,
500                count: 1,
501            },
502            PointField {
503                name: "i32".to_string(),
504                offset: 6,
505                datatype: point_field::INT32,
506                count: 1,
507            },
508            PointField {
509                name: "u32".to_string(),
510                offset: 10,
511                datatype: point_field::UINT32,
512                count: 1,
513            },
514            PointField {
515                name: "f32".to_string(),
516                offset: 14,
517                datatype: point_field::FLOAT32,
518                count: 1,
519            },
520            PointField {
521                name: "f64".to_string(),
522                offset: 18,
523                datatype: point_field::FLOAT64,
524                count: 1,
525            },
526        ];
527
528        let mut data = Vec::new();
529        data.extend_from_slice(&(-100i8).to_le_bytes());
530        data.extend_from_slice(&200u8.to_le_bytes());
531        data.extend_from_slice(&(-1000i16).to_le_bytes());
532        data.extend_from_slice(&50000u16.to_le_bytes());
533        data.extend_from_slice(&(-100000i32).to_le_bytes());
534        data.extend_from_slice(&3000000000u32.to_le_bytes());
535        data.extend_from_slice(&std::f32::consts::PI.to_le_bytes());
536        data.extend_from_slice(&std::f64::consts::E.to_le_bytes());
537
538        let cloud = PointCloud2 {
539            header: Header {
540                stamp: Time::new(0, 0),
541                frame_id: String::new(),
542            },
543            height: 1,
544            width: 1,
545            fields,
546            is_bigendian: false,
547            point_step: 26,
548            row_step: 26,
549            data,
550            is_dense: true,
551        };
552
553        let points = decode_pcd(&cloud);
554        assert_eq!(points.len(), 1);
555        let p = &points[0];
556        assert!((p.fields["i8"] - (-100.0)).abs() < 1e-6);
557        assert!((p.fields["u8"] - 200.0).abs() < 1e-6);
558        assert!((p.fields["i16"] - (-1000.0)).abs() < 1e-6);
559        assert!((p.fields["u16"] - 50000.0).abs() < 1e-6);
560        assert!((p.fields["i32"] - (-100000.0)).abs() < 1e-6);
561        assert!((p.fields["u32"] - 3000000000.0).abs() < 1e-6);
562        assert!((p.fields["f32"] - std::f32::consts::PI as f64).abs() < 1e-6);
563        assert!((p.fields["f64"] - std::f64::consts::E).abs() < 1e-9);
564    }
565
566    #[test]
567    fn decode_pcd_all_datatypes_big_endian() {
568        let fields = vec![
569            PointField {
570                name: "i16".to_string(),
571                offset: 0,
572                datatype: point_field::INT16,
573                count: 1,
574            },
575            PointField {
576                name: "u32".to_string(),
577                offset: 2,
578                datatype: point_field::UINT32,
579                count: 1,
580            },
581            PointField {
582                name: "f64".to_string(),
583                offset: 6,
584                datatype: point_field::FLOAT64,
585                count: 1,
586            },
587        ];
588
589        let mut data = Vec::new();
590        data.extend_from_slice(&(-500i16).to_be_bytes());
591        data.extend_from_slice(&123456789u32.to_be_bytes());
592        data.extend_from_slice(&1.23456789f64.to_be_bytes());
593
594        let cloud = PointCloud2 {
595            header: Header {
596                stamp: Time::new(0, 0),
597                frame_id: String::new(),
598            },
599            height: 1,
600            width: 1,
601            fields,
602            is_bigendian: true,
603            point_step: 14,
604            row_step: 14,
605            data,
606            is_dense: true,
607        };
608
609        let points = decode_pcd(&cloud);
610        let p = &points[0];
611        assert!((p.fields["i16"] - (-500.0)).abs() < 1e-6);
612        assert!((p.fields["u32"] - 123456789.0).abs() < 1e-6);
613        assert!((p.fields["f64"] - 1.23456789).abs() < 1e-9);
614    }
615
616    #[test]
617    fn decode_pcd_unknown_datatype_skipped() {
618        // Unknown datatype (99) should be silently skipped
619        let fields = vec![
620            PointField {
621                name: "x".to_string(),
622                offset: 0,
623                datatype: point_field::FLOAT32,
624                count: 1,
625            },
626            PointField {
627                name: "unknown".to_string(),
628                offset: 4,
629                datatype: 99, // Invalid datatype
630                count: 1,
631            },
632        ];
633
634        let mut data = Vec::new();
635        data.extend_from_slice(&42.0f32.to_le_bytes());
636        data.extend_from_slice(&[0u8; 4]); // Padding for unknown field
637
638        let cloud = PointCloud2 {
639            header: Header {
640                stamp: Time::new(0, 0),
641                frame_id: String::new(),
642            },
643            height: 1,
644            width: 1,
645            fields,
646            is_bigendian: false,
647            point_step: 8,
648            row_step: 8,
649            data,
650            is_dense: true,
651        };
652
653        let points = decode_pcd(&cloud);
654        assert_eq!(points.len(), 1);
655        assert!((points[0].x - 42.0).abs() < 1e-6);
656        // Unknown field should not be in the map
657        assert!(!points[0].fields.contains_key("unknown"));
658    }
659
660    #[test]
661    fn decode_pcd_multi_row() {
662        // 2x3 organized point cloud (2 rows, 3 columns)
663        let input = [
664            [1.0f32, 1.0, 1.0],
665            [2.0, 2.0, 2.0],
666            [3.0, 3.0, 3.0],
667            [4.0, 4.0, 4.0],
668            [5.0, 5.0, 5.0],
669            [6.0, 6.0, 6.0],
670        ];
671
672        let fields = vec![
673            PointField {
674                name: "x".to_string(),
675                offset: 0,
676                datatype: point_field::FLOAT32,
677                count: 1,
678            },
679            PointField {
680                name: "y".to_string(),
681                offset: 4,
682                datatype: point_field::FLOAT32,
683                count: 1,
684            },
685            PointField {
686                name: "z".to_string(),
687                offset: 8,
688                datatype: point_field::FLOAT32,
689                count: 1,
690            },
691        ];
692
693        let mut data = Vec::new();
694        for p in &input {
695            for val in p {
696                data.extend_from_slice(&val.to_le_bytes());
697            }
698        }
699
700        let cloud = PointCloud2 {
701            header: Header {
702                stamp: Time::new(0, 0),
703                frame_id: "camera".to_string(),
704            },
705            height: 2,
706            width: 3,
707            fields,
708            is_bigendian: false,
709            point_step: 12,
710            row_step: 36, // 3 points * 12 bytes
711            data,
712            is_dense: true,
713        };
714
715        let points = decode_pcd(&cloud);
716        assert_eq!(points.len(), 6);
717        for (i, p) in points.iter().enumerate() {
718            let expected = (i + 1) as f64;
719            assert!((p.x - expected).abs() < 1e-6, "point {} x mismatch", i);
720        }
721    }
722}
edgefirst_schemas/lib.rs

edgefirst_schemas/
lib.rs
