edgefirst-schemas 3.1.0

Message schemas for EdgeFirst Perception - ROS2 Common Interfaces, Foxglove, and custom types
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129

// SPDX-License-Identifier: Apache-2.0
// Copyright © 2026 Au-Zone Technologies. All Rights Reserved.

//! ROS 2 `nav_msgs` message types.
//!
//! Buffer-backed: `Odometry`

use crate::builtin_interfaces::Time;
use crate::cdr::*;
use crate::geometry_msgs::{PoseWithCovariance, TwistWithCovariance};
use crate::std_msgs::Header;

// ── Odometry<B> ─────────────────────────────────────────────────────
//
// CDR layout: Header → pre, then:
//   string child_frame_id → offsets[0] (start of child_frame_id)
//   pad to 8 → offsets[1] (start of PoseWithCovariance)
//   PoseWithCovariance pose  → offsets[2] captured after pose decode,
//                              i.e. start of TwistWithCovariance
//   TwistWithCovariance twist
//
// offsets[1] is captured from the cursor after aligning to 8 because
// `child_frame_id` is a variable-length string; PoseWithCovariance
// starts with a Point (f64 first), so an explicit align-to-8 is needed.
// offsets[2] is captured from the cursor after pose decode so `twist()`
// doesn't depend on `PoseWithCovariance::CDR_SIZE` layout arithmetic.

pub struct Odometry<B> {
    buf: B,
    offsets: [usize; 3],
}

impl<B: AsRef<[u8]>> Odometry<B> {
    pub fn from_cdr(buf: B) -> Result<Self, CdrError> {
        let header = Header::<&[u8]>::from_cdr(buf.as_ref())?;
        let pre = header.end_offset();
        let mut c = CdrCursor::resume(buf.as_ref(), pre);
        let o0 = c.offset();
        let _ = c.read_string()?; // child_frame_id
        c.align(8);
        let o1 = c.offset();
        PoseWithCovariance::read_cdr(&mut c)?;
        let o2 = c.offset();
        TwistWithCovariance::read_cdr(&mut c)?;
        Ok(Odometry {
            offsets: [o0, o1, o2],
            buf,
        })
    }

    /// Returns a `Header` view by re-parsing the CDR buffer prefix.
    pub fn header(&self) -> Header<&[u8]> {
        Header::from_cdr(self.buf.as_ref()).expect("header bytes validated during from_cdr")
    }
    pub fn stamp(&self) -> Time {
        rd_time(self.buf.as_ref(), CDR_HEADER_SIZE)
    }
    pub fn frame_id(&self) -> &str {
        rd_string(self.buf.as_ref(), CDR_HEADER_SIZE + 8).0
    }
    pub fn child_frame_id(&self) -> &str {
        rd_string(self.buf.as_ref(), self.offsets[0]).0
    }
    pub fn pose(&self) -> PoseWithCovariance {
        let mut c = CdrCursor::resume(self.buf.as_ref(), self.offsets[1]);
        PoseWithCovariance::read_cdr(&mut c).expect("pose validated during from_cdr")
    }
    pub fn twist(&self) -> TwistWithCovariance {
        let mut c = CdrCursor::resume(self.buf.as_ref(), self.offsets[2]);
        TwistWithCovariance::read_cdr(&mut c).expect("twist validated during from_cdr")
    }
    pub fn as_cdr(&self) -> &[u8] {
        self.buf.as_ref()
    }
    pub fn to_cdr(&self) -> Vec<u8> {
        self.buf.as_ref().to_vec()
    }
}

impl Odometry<Vec<u8>> {
    pub fn new(
        stamp: Time,
        frame_id: &str,
        child_frame_id: &str,
        pose: PoseWithCovariance,
        twist: TwistWithCovariance,
    ) -> Result<Self, CdrError> {
        let mut sizer = CdrSizer::new();
        Time::size_cdr(&mut sizer);
        sizer.size_string(frame_id);
        let o0 = sizer.offset();
        sizer.size_string(child_frame_id);
        sizer.align(8);
        let o1 = sizer.offset();
        PoseWithCovariance::size_cdr(&mut sizer);
        let o2 = sizer.offset();
        TwistWithCovariance::size_cdr(&mut sizer);

        let mut buf = vec![0u8; sizer.size()];
        let mut w = CdrWriter::new(&mut buf)?;
        stamp.write_cdr(&mut w);
        w.write_string(frame_id);
        w.write_string(child_frame_id);
        pose.write_cdr(&mut w);
        twist.write_cdr(&mut w);
        w.finish()?;

        Ok(Odometry {
            offsets: [o0, o1, o2],
            buf,
        })
    }

    pub fn into_cdr(self) -> Vec<u8> {
        self.buf
    }
}

// ── Registry ────────────────────────────────────────────────────────

/// Check if a type name is supported by this module.
pub fn is_type_supported(type_name: &str) -> bool {
    matches!(type_name, "Odometry")
}

/// List all type schema names in this module.
pub fn list_types() -> &'static [&'static str] {
    &["nav_msgs/msg/Odometry"]
}