nros_node/
action_frame.rs

1//! Wire framing of the action messages an action CLIENT takes off the wire —
2//! the ONE place their payload offsets are spelled (issue 2014).
3//!
4//! The producer side (`action_core::write_get_result_header`,
5//! `publish_feedback_raw`) writes these frames with a [`CdrWriter`], so its
6//! offsets fall out of `align()` and are never written down. Every CONSUMER
7//! used to spell them itself — the arena dispatch, the typed dispatch, the C
8//! and C++ polling paths, the C++ `PollingActionClient` header — and they
9//! disagreed: the header skipped 5 bytes of a get_result reply whose payload
10//! starts at 8, and the C poll path handed feedback fields to a callback with
11//! no encapsulation header while the arena handed it one.
12//!
13//! Frames (all little-endian CDR, top-level encapsulation header first):
14//!
15//! * `SendGoal` reply: `[encap 4][accepted u8][stamp …]`
16//! * `GetResult` reply: `[encap 4][status i8][pad to 4 → byte 8][result fields]`
17//! * `FeedbackMessage`: `[encap 4][GoalUuid.uuid 16][feedback fields]`
18//! * `SendGoal` request (server side): `[encap 4][GoalUuid.uuid 16][goal fields]`
19//! * `CancelGoal` reply: `[encap 4][return_code i8][pad to 4 → byte 8][count u32]
20//!   [GoalInfo { uuid 16, stamp.sec i32, stamp.nanosec u32 } × count]`
21//!
22//! The payload fields arrive WITHOUT an encapsulation header of their own on
23//! every backend (RFC-0069 / issue 0418), and every deserializer reads with
24//! `new_with_header`, so a consumer [`splice`]s the frame's own top-level
25//! header in front of them. That is the shape every callback receives.
26//!
27//! [`CdrWriter`]: nros_core::CdrWriter
28//! [`splice`]: crate::action_frame::splice
29
30use nros_core::{GoalStatusCode, GoalUuid};
31
32/// Length of a CDR encapsulation header.
33pub const CDR_HEADER_LEN: usize = nros_serdes::CDR_HEADER_LEN;
34
35/// Byte holding the `accepted` flag of a `SendGoal` reply and the `status` of
36/// a `GetResult` reply — the first field after the encapsulation header.
37pub const REPLY_STATUS_OFFSET: usize = CDR_HEADER_LEN;
38
39/// First byte of the result fields in a `GetResult` reply: the `i8` status,
40/// then CDR alignment to 4 (stream position 4 = byte 8).
41pub const RESULT_PAYLOAD_OFFSET: usize = (REPLY_STATUS_OFFSET + 1 + 3) & !3;
42
43/// First byte of the feedback fields in a `FeedbackMessage`: the GoalUuid UUID
44/// (`uint8[16]`, no length prefix since 233.6) follows the header.
45pub const FEEDBACK_PAYLOAD_OFFSET: usize =
46    CDR_HEADER_LEN + GoalUuid::SEQ_PREFIX_LEN + GoalUuid::UUID_LEN;
47
48/// First byte of the goal fields in a `SendGoal` request — the same framing
49/// as feedback: the GoalUuid follows the header.
50pub const GOAL_REQUEST_PAYLOAD_OFFSET: usize = FEEDBACK_PAYLOAD_OFFSET;
51
52/// Byte holding the `goals_canceling` sequence length of a `CancelGoal`
53/// reply: the `i8` return code, then CDR alignment to 4 (issue 2027).
54pub const CANCEL_GOALS_COUNT_OFFSET: usize = (REPLY_STATUS_OFFSET + 1 + 3) & !3;
55
56/// First `GoalInfo` of a `CancelGoal` reply's `goals_canceling`.
57pub const CANCEL_GOALS_OFFSET: usize = CANCEL_GOALS_COUNT_OFFSET + 4;
58
59/// One `action_msgs/msg/GoalInfo` on the wire: the UUID (`uint8[16]`), then
60/// `builtin_interfaces/Time` (`int32 sec`, `uint32 nanosec`, 4-aligned, which
61/// 16 bytes in already is).
62pub const GOAL_INFO_LEN: usize = GoalUuid::UUID_LEN + 4 + 4;
63
64const _: () = assert!(RESULT_PAYLOAD_OFFSET == 8);
65const _: () = assert!(CANCEL_GOALS_OFFSET == 12);
66const _: () = assert!(FEEDBACK_PAYLOAD_OFFSET == 20);
67
68/// The `accepted` flag of a `SendGoal` reply (`false` for a short frame).
69pub fn goal_accepted(reply: &[u8]) -> bool {
70    reply.len() > REPLY_STATUS_OFFSET && reply[REPLY_STATUS_OFFSET] != 0
71}
72
73/// The goal UUID a `FeedbackMessage` names (`None` for a short frame).
74pub fn feedback_goal_id(msg: &[u8]) -> Option<GoalUuid> {
75    let bytes = msg.get(CDR_HEADER_LEN..FEEDBACK_PAYLOAD_OFFSET)?;
76    let mut uuid = [0u8; GoalUuid::UUID_LEN];
77    uuid.copy_from_slice(bytes);
78    Some(GoalUuid { uuid })
79}
80
81/// The terminal status of a `GetResult` reply, or `None` when the frame is
82/// too short to hold a result (shorter than [`RESULT_PAYLOAD_OFFSET`]).
83/// Anything but Succeeded / Canceled / Aborted reads as `Unknown`.
84pub fn result_status(reply: &[u8]) -> Option<GoalStatusCode> {
85    if reply.len() < RESULT_PAYLOAD_OFFSET {
86        return None;
87    }
88    Some(match reply[REPLY_STATUS_OFFSET] {
89        4 => GoalStatusCode::Succeeded,
90        5 => GoalStatusCode::Canceled,
91        6 => GoalStatusCode::Aborted,
92        _ => GoalStatusCode::Unknown,
93    })
94}
95
96/// The result fields of a `GetResult` reply, without any header (`None` when
97/// the frame is shorter than [`RESULT_PAYLOAD_OFFSET`]).
98pub fn result_fields(reply: &[u8]) -> Option<&[u8]> {
99    reply.get(RESULT_PAYLOAD_OFFSET..)
100}
101
102/// The feedback fields of a `FeedbackMessage`, without any header. `None`
103/// when the frame carries no fields at all — an empty feedback is not
104/// delivered, which is what every consumer did before this helper existed.
105pub fn feedback_fields(msg: &[u8]) -> Option<&[u8]> {
106    if msg.len() <= FEEDBACK_PAYLOAD_OFFSET {
107        return None;
108    }
109    msg.get(FEEDBACK_PAYLOAD_OFFSET..)
110}
111
112/// The goal fields of a `SendGoal` request, without any header (`None` when
113/// the frame carries no fields).
114pub fn goal_request_fields(req: &[u8]) -> Option<&[u8]> {
115    if req.len() <= GOAL_REQUEST_PAYLOAD_OFFSET {
116        return None;
117    }
118    req.get(GOAL_REQUEST_PAYLOAD_OFFSET..)
119}
120
121/// The `return_code` of a `CancelGoal` reply (`None` for a short frame).
122pub fn cancel_return_code(reply: &[u8]) -> Option<i8> {
123    reply.get(REPLY_STATUS_OFFSET).map(|b| *b as i8)
124}
125
126/// How many goals a `CancelGoal` reply says are canceling — issue 2027.
127/// `None` when the frame is too short to hold the count, or the count names
128/// more `GoalInfo`s than the frame carries (a truncation is not "none").
129pub fn cancel_goals_count(reply: &[u8]) -> Option<usize> {
130    let raw = reply.get(CANCEL_GOALS_COUNT_OFFSET..CANCEL_GOALS_OFFSET)?;
131    let n = u32::from_le_bytes([raw[0], raw[1], raw[2], raw[3]]) as usize;
132    let need = n
133        .checked_mul(GOAL_INFO_LEN)?
134        .checked_add(CANCEL_GOALS_OFFSET)?;
135    (reply.len() >= need).then_some(n)
136}
137
138/// The `i`-th `GoalInfo` of a `CancelGoal` reply's `goals_canceling`: the
139/// goal and its acceptance stamp `(sec, nanosec)`. `None` past the count (or
140/// on a short frame).
141pub fn cancel_goal_info(reply: &[u8], i: usize) -> Option<(GoalUuid, i32, u32)> {
142    if i >= cancel_goals_count(reply)? {
143        return None;
144    }
145    let at = CANCEL_GOALS_OFFSET + i * GOAL_INFO_LEN;
146    let b = reply.get(at..at + GOAL_INFO_LEN)?;
147    let mut uuid = [0u8; GoalUuid::UUID_LEN];
148    uuid.copy_from_slice(&b[..GoalUuid::UUID_LEN]);
149    let s = GoalUuid::UUID_LEN;
150    let sec = i32::from_le_bytes([b[s], b[s + 1], b[s + 2], b[s + 3]]);
151    let nanosec = u32::from_le_bytes([b[s + 4], b[s + 5], b[s + 6], b[s + 7]]);
152    Some((GoalUuid { uuid }, sec, nanosec))
153}
154
155/// Write `frame`'s own encapsulation header followed by `fields` into `out`,
156/// truncating `fields` to what fits. Returns the bytes written, or `None` when
157/// `frame` has no header or `out` cannot hold one.
158pub fn splice(frame: &[u8], fields: &[u8], out: &mut [u8]) -> Option<usize> {
159    if frame.len() < CDR_HEADER_LEN || out.len() < CDR_HEADER_LEN {
160        return None;
161    }
162    let n = fields.len().min(out.len() - CDR_HEADER_LEN);
163    out[..CDR_HEADER_LEN].copy_from_slice(&frame[..CDR_HEADER_LEN]);
164    out[CDR_HEADER_LEN..CDR_HEADER_LEN + n].copy_from_slice(&fields[..n]);
165    Some(CDR_HEADER_LEN + n)
166}
167
168/// A `GetResult` reply's result, header-framed for a deserializer.
169pub fn splice_result(reply: &[u8], out: &mut [u8]) -> Option<usize> {
170    splice(reply, result_fields(reply)?, out)
171}
172
173/// A `FeedbackMessage`'s feedback, header-framed for a deserializer.
174pub fn splice_feedback(msg: &[u8], out: &mut [u8]) -> Option<usize> {
175    splice(msg, feedback_fields(msg)?, out)
176}
177
178/// A `SendGoal` request's goal, header-framed for a deserializer.
179pub fn splice_goal_request(req: &[u8], out: &mut [u8]) -> Option<usize> {
180    splice(req, goal_request_fields(req)?, out)
181}
182
183#[cfg(test)]
184mod tests {
185    use super::*;
186
187    #[test]
188    fn result_payload_starts_after_the_alignment_pad() {
189        // [encap][status=4][pad pad pad][fields 0xAA 0xBB]
190        let reply = [0, 1, 0, 0, 4, 0xEE, 0xEE, 0xEE, 0xAA, 0xBB];
191        assert_eq!(result_status(&reply), Some(GoalStatusCode::Succeeded));
192        assert_eq!(result_fields(&reply), Some(&[0xAA, 0xBB][..]));
193        let mut out = [0u8; 16];
194        let n = splice_result(&reply, &mut out).unwrap();
195        assert_eq!(&out[..n], &[0, 1, 0, 0, 0xAA, 0xBB]);
196    }
197
198    #[test]
199    fn cancel_reply_lists_its_canceling_goals() {
200        // [encap][code=0][pad][count=2][uuid 16, sec, nsec] x 2
201        let mut reply = [0u8; CANCEL_GOALS_OFFSET + 2 * GOAL_INFO_LEN];
202        reply[..4].copy_from_slice(&[0, 1, 0, 0]);
203        reply[5..8].copy_from_slice(&[0xEE; 3]);
204        reply[8..12].copy_from_slice(&2u32.to_le_bytes());
205        reply[12] = 9;
206        reply[28..32].copy_from_slice(&5i32.to_le_bytes());
207        reply[32..36].copy_from_slice(&7u32.to_le_bytes());
208        reply[36] = 10;
209        assert_eq!(cancel_return_code(&reply), Some(0));
210        assert_eq!(cancel_goals_count(&reply), Some(2));
211        let (g, sec, ns) = cancel_goal_info(&reply, 0).unwrap();
212        assert_eq!((g.uuid[0], sec, ns), (9, 5, 7));
213        assert_eq!(cancel_goal_info(&reply, 1).unwrap().0.uuid[0], 10);
214        assert!(cancel_goal_info(&reply, 2).is_none());
215        // A count naming more entries than the frame holds is a truncation.
216        assert_eq!(cancel_goals_count(&reply[..CANCEL_GOALS_OFFSET + 4]), None);
217        assert_eq!(cancel_goals_count(&[0, 1, 0, 0, 0]), None);
218    }
219
220    #[test]
221    fn short_frames_carry_nothing() {
222        assert_eq!(result_status(&[0, 1, 0, 0, 4]), None);
223        assert_eq!(feedback_fields(&[0u8; FEEDBACK_PAYLOAD_OFFSET]), None);
224        assert!(!goal_accepted(&[0, 1, 0, 0]));
225    }
226
227    #[test]
228    fn feedback_splices_the_frames_own_header() {
229        let mut msg = [0u8; FEEDBACK_PAYLOAD_OFFSET + 2];
230        msg[..4].copy_from_slice(&[0, 0, 0, 0]); // big-endian encap
231        msg[4] = 7; // first uuid byte
232        msg[FEEDBACK_PAYLOAD_OFFSET..].copy_from_slice(&[1, 2]);
233        assert_eq!(feedback_goal_id(&msg).unwrap().uuid[0], 7);
234        let mut out = [0u8; 8];
235        let n = splice_feedback(&msg, &mut out).unwrap();
236        assert_eq!(&out[..n], &[0, 0, 0, 0, 1, 2]);
237    }
238}
nros_node/action_frame.rs

nros_node/
action_frame.rs
