franka/robot/
logger.rs

1//! Ring buffer of the last robot states and commands of a motion.
2//!
3//! Port of `franka::RobotStateLogger` (libfranka 0.21.2 `src/logging/robot_state_logger.cpp`).
4//! The buffer is filled by every control cycle and flushed into the [`ControlException`]'s
5//! `log` field when a motion ends abnormally.
6//!
7//! [`ControlException`]: crate::error::ControlException
8
9use crate::error::{Record, RobotCommandLog};
10use crate::robot_state::RobotState;
11
12/// Fixed-capacity ring buffer of `(state, command)` pairs.
13#[derive(Debug)]
14pub(crate) struct RobotStateLogger {
15    log_size: usize,
16    states: Vec<RobotState>,
17    commands: Vec<RobotCommandLog>,
18    ring_front: usize,
19    ring_size: usize,
20}
21
22impl RobotStateLogger {
23    /// Creates a logger holding at most `log_size` entries. A size of `0` disables logging.
24    pub(crate) fn new(log_size: usize) -> RobotStateLogger {
25        RobotStateLogger {
26            log_size,
27            states: vec![RobotState::default(); log_size],
28            commands: vec![RobotCommandLog::default(); log_size],
29            ring_front: 0,
30            ring_size: 0,
31        }
32    }
33
34    /// Records one cycle (`RobotStateLogger::log`).
35    ///
36    /// Never allocates: the vectors are sized once in [`RobotStateLogger::new`].
37    ///
38    /// The command is the version-agnostic [`RobotCommandLog`] rather than a wire
39    /// `RobotCommand`, because the wire struct differs between FCI v5 and v10 while the log
40    /// (which ends up in a user-visible [`crate::error::ControlException`]) must not.
41    pub(crate) fn log(&mut self, state: &RobotState, command: &RobotCommandLog) {
42        if self.log_size == 0 {
43            return;
44        }
45
46        self.states[self.ring_front] = *state;
47        self.commands[self.ring_front] = *command;
48
49        self.ring_front = (self.ring_front + 1) % self.log_size;
50        self.ring_size = self.log_size.min(self.ring_size + 1);
51    }
52
53    /// Returns the recorded cycles oldest first and empties the buffer
54    /// (`RobotStateLogger::flush`).
55    pub(crate) fn flush(&mut self) -> Vec<Record> {
56        let mut log = Vec::with_capacity(self.ring_size);
57        for i in 0..self.ring_size {
58            // Identical to the C++ `(ring_front_ + i) % ring_size_`: while the buffer is not
59            // full `ring_front_ == ring_size_`, so both expressions yield `i`.
60            let wrapped = (self.ring_front + i) % self.ring_size;
61            log.push(Record {
62                state: self.states[wrapped],
63                command: Some(self.commands[wrapped]),
64            });
65        }
66        self.ring_front = 0;
67        self.ring_size = 0;
68        log
69    }
70}
71
72#[cfg(test)]
73mod tests {
74    use super::*;
75    use crate::duration::Duration;
76
77    fn state(time_ms: u64) -> RobotState {
78        RobotState {
79            time: Duration::from_millis(time_ms),
80            ..RobotState::default()
81        }
82    }
83
84    #[test]
85    fn zero_size_logger_records_nothing() {
86        let mut logger = RobotStateLogger::new(0);
87        logger.log(&state(1), &RobotCommandLog::default());
88        assert!(logger.flush().is_empty());
89    }
90
91    #[test]
92    fn ring_buffer_keeps_the_newest_entries_in_order() {
93        let mut logger = RobotStateLogger::new(3);
94        for t in 1..=5 {
95            logger.log(&state(t), &RobotCommandLog::default());
96        }
97        let log = logger.flush();
98        let times: Vec<u64> = log.iter().map(|r| r.state.time.as_millis()).collect();
99        assert_eq!(times, vec![3, 4, 5]);
100        // Flushing empties the buffer.
101        assert!(logger.flush().is_empty());
102    }
103
104    #[test]
105    fn partially_filled_buffer_is_ordered() {
106        let mut logger = RobotStateLogger::new(4);
107        for t in 1..=2 {
108            logger.log(&state(t), &RobotCommandLog::default());
109        }
110        let times: Vec<u64> = logger
111            .flush()
112            .iter()
113            .map(|r| r.state.time.as_millis())
114            .collect();
115        assert_eq!(times, vec![1, 2]);
116    }
117}
franka/robot/logger.rs

franka/robot/
logger.rs
