piper_driver/
fps_stats.rs

1//! FPS 统计模块
2//!
3//! 用于统计各个状态的更新频率（Frames Per Second），用于性能监控和调试诊断。
4
5use std::sync::atomic::{AtomicU64, Ordering};
6use std::time::Instant;
7
8/// FPS 统计数据
9///
10/// 使用原子计数器记录各状态的更新次数，支持无锁读取。
11/// 使用固定时间窗口统计 FPS，从创建或重置开始计算。
12#[derive(Debug)]
13pub struct FpsStatistics {
14    // 热数据更新计数器（原子操作，无锁）
15    pub(crate) joint_position_updates: AtomicU64,
16    pub(crate) end_pose_updates: AtomicU64,
17    pub(crate) joint_dynamic_updates: AtomicU64,
18
19    // 温数据更新计数器
20    pub(crate) robot_control_updates: AtomicU64,
21    pub(crate) gripper_updates: AtomicU64,
22
23    // 温数据更新计数器（40Hz诊断数据）
24    pub(crate) joint_driver_low_speed_updates: AtomicU64,
25
26    // 冷数据更新计数器
27    pub(crate) collision_protection_updates: AtomicU64,
28    pub(crate) joint_limit_config_updates: AtomicU64,
29    pub(crate) joint_accel_config_updates: AtomicU64,
30    pub(crate) end_limit_config_updates: AtomicU64,
31    pub(crate) firmware_version_updates: AtomicU64,
32
33    // 主从模式控制指令更新计数器
34    pub(crate) master_slave_control_mode_updates: AtomicU64,
35    pub(crate) master_slave_joint_control_updates: AtomicU64,
36    pub(crate) master_slave_gripper_control_updates: AtomicU64,
37
38    // 统计窗口开始时间
39    pub(crate) window_start: Instant,
40}
41
42impl FpsStatistics {
43    /// 创建新的 FPS 统计实例
44    pub fn new() -> Self {
45        Self {
46            joint_position_updates: AtomicU64::new(0),
47            end_pose_updates: AtomicU64::new(0),
48            joint_dynamic_updates: AtomicU64::new(0),
49            robot_control_updates: AtomicU64::new(0),
50            gripper_updates: AtomicU64::new(0),
51            joint_driver_low_speed_updates: AtomicU64::new(0),
52            collision_protection_updates: AtomicU64::new(0),
53            joint_limit_config_updates: AtomicU64::new(0),
54            joint_accel_config_updates: AtomicU64::new(0),
55            end_limit_config_updates: AtomicU64::new(0),
56            firmware_version_updates: AtomicU64::new(0),
57            master_slave_control_mode_updates: AtomicU64::new(0),
58            master_slave_joint_control_updates: AtomicU64::new(0),
59            master_slave_gripper_control_updates: AtomicU64::new(0),
60            window_start: Instant::now(),
61        }
62    }
63
64    /// 重置统计窗口
65    ///
66    /// 清除当前计数器并开始新的统计窗口。
67    /// 注意：此方法需要可变引用，如果需要从不可变上下文重置，
68    /// 可以考虑使用 `Arc<Mutex<FpsStatistics>>` 或提供重置方法返回新实例。
69    pub fn reset(&mut self) {
70        self.joint_position_updates.store(0, Ordering::Relaxed);
71        self.end_pose_updates.store(0, Ordering::Relaxed);
72        self.joint_dynamic_updates.store(0, Ordering::Relaxed);
73        self.robot_control_updates.store(0, Ordering::Relaxed);
74        self.gripper_updates.store(0, Ordering::Relaxed);
75        self.joint_driver_low_speed_updates.store(0, Ordering::Relaxed);
76        self.collision_protection_updates.store(0, Ordering::Relaxed);
77        self.joint_limit_config_updates.store(0, Ordering::Relaxed);
78        self.joint_accel_config_updates.store(0, Ordering::Relaxed);
79        self.end_limit_config_updates.store(0, Ordering::Relaxed);
80        self.firmware_version_updates.store(0, Ordering::Relaxed);
81        self.master_slave_control_mode_updates.store(0, Ordering::Relaxed);
82        self.master_slave_joint_control_updates.store(0, Ordering::Relaxed);
83        self.master_slave_gripper_control_updates.store(0, Ordering::Relaxed);
84        self.window_start = Instant::now();
85    }
86
87    /// 计算 FPS（基于当前计数器和时间窗口）
88    ///
89    /// 返回从统计窗口开始到现在各状态的更新频率（FPS）。
90    ///
91    /// # 性能
92    /// - 无锁读取（仅原子读取）
93    /// - 开销：~100ns（5 次原子读取 + 浮点计算）
94    pub fn calculate_fps(&self) -> FpsResult {
95        let elapsed_secs = self.window_start.elapsed().as_secs_f64();
96
97        // 避免除零（至少 1ms）
98        let elapsed_secs = elapsed_secs.max(0.001);
99
100        FpsResult {
101            joint_position: self.joint_position_updates.load(Ordering::Relaxed) as f64
102                / elapsed_secs,
103            end_pose: self.end_pose_updates.load(Ordering::Relaxed) as f64 / elapsed_secs,
104            joint_dynamic: self.joint_dynamic_updates.load(Ordering::Relaxed) as f64 / elapsed_secs,
105            robot_control: self.robot_control_updates.load(Ordering::Relaxed) as f64 / elapsed_secs,
106            gripper: self.gripper_updates.load(Ordering::Relaxed) as f64 / elapsed_secs,
107            joint_driver_low_speed: self.joint_driver_low_speed_updates.load(Ordering::Relaxed)
108                as f64
109                / elapsed_secs,
110            collision_protection: self.collision_protection_updates.load(Ordering::Relaxed) as f64
111                / elapsed_secs,
112            joint_limit_config: self.joint_limit_config_updates.load(Ordering::Relaxed) as f64
113                / elapsed_secs,
114            joint_accel_config: self.joint_accel_config_updates.load(Ordering::Relaxed) as f64
115                / elapsed_secs,
116            end_limit_config: self.end_limit_config_updates.load(Ordering::Relaxed) as f64
117                / elapsed_secs,
118            firmware_version: self.firmware_version_updates.load(Ordering::Relaxed) as f64
119                / elapsed_secs,
120            master_slave_control_mode: self
121                .master_slave_control_mode_updates
122                .load(Ordering::Relaxed) as f64
123                / elapsed_secs,
124            master_slave_joint_control: self
125                .master_slave_joint_control_updates
126                .load(Ordering::Relaxed) as f64
127                / elapsed_secs,
128            master_slave_gripper_control: self
129                .master_slave_gripper_control_updates
130                .load(Ordering::Relaxed) as f64
131                / elapsed_secs,
132        }
133    }
134
135    /// 获取原始计数器值（用于精确计算）
136    ///
137    /// 返回当前各状态的更新计数，可以配合自定义时间窗口计算 FPS。
138    ///
139    /// # 性能
140    /// - 无锁读取（仅原子读取）
141    /// - 开销：~50ns（5 次原子读取）
142    pub fn get_counts(&self) -> FpsCounts {
143        FpsCounts {
144            joint_position: self.joint_position_updates.load(Ordering::Relaxed),
145            end_pose: self.end_pose_updates.load(Ordering::Relaxed),
146            joint_dynamic: self.joint_dynamic_updates.load(Ordering::Relaxed),
147            robot_control: self.robot_control_updates.load(Ordering::Relaxed),
148            gripper: self.gripper_updates.load(Ordering::Relaxed),
149            joint_driver_low_speed: self.joint_driver_low_speed_updates.load(Ordering::Relaxed),
150            collision_protection: self.collision_protection_updates.load(Ordering::Relaxed),
151            joint_limit_config: self.joint_limit_config_updates.load(Ordering::Relaxed),
152            joint_accel_config: self.joint_accel_config_updates.load(Ordering::Relaxed),
153            end_limit_config: self.end_limit_config_updates.load(Ordering::Relaxed),
154            firmware_version: self.firmware_version_updates.load(Ordering::Relaxed),
155            master_slave_control_mode: self
156                .master_slave_control_mode_updates
157                .load(Ordering::Relaxed),
158            master_slave_joint_control: self
159                .master_slave_joint_control_updates
160                .load(Ordering::Relaxed),
161            master_slave_gripper_control: self
162                .master_slave_gripper_control_updates
163                .load(Ordering::Relaxed),
164        }
165    }
166
167    /// 获取统计窗口开始时间
168    pub fn window_start(&self) -> Instant {
169        self.window_start
170    }
171
172    /// 获取统计窗口经过的时间
173    pub fn elapsed(&self) -> std::time::Duration {
174        self.window_start.elapsed()
175    }
176}
177
178impl Default for FpsStatistics {
179    fn default() -> Self {
180        Self::new()
181    }
182}
183
184/// FPS 计算结果
185///
186/// 包含各状态的更新频率（FPS）。
187#[derive(Debug, Clone, Copy)]
188pub struct FpsResult {
189    /// 关节位置状态 FPS（预期：~500Hz）
190    pub joint_position: f64,
191    /// 末端位姿状态 FPS（预期：~500Hz）
192    pub end_pose: f64,
193    /// 关节动态状态 FPS（预期：~500Hz）
194    pub joint_dynamic: f64,
195    /// 机器人控制状态 FPS（预期：~200Hz）
196    pub robot_control: f64,
197    /// 夹爪状态 FPS（预期：~200Hz）
198    pub gripper: f64,
199    /// 关节驱动器低速反馈状态 FPS（预期：~40Hz）
200    pub joint_driver_low_speed: f64,
201    /// 碰撞保护状态 FPS（预期：按需查询）
202    pub collision_protection: f64,
203    /// 关节限制配置状态 FPS（预期：按需查询）
204    pub joint_limit_config: f64,
205    /// 关节加速度限制配置状态 FPS（预期：按需查询）
206    pub joint_accel_config: f64,
207    /// 末端限制配置状态 FPS（预期：按需查询）
208    pub end_limit_config: f64,
209    /// 固件版本状态 FPS（预期：按需查询）
210    pub firmware_version: f64,
211    /// 主从模式控制模式指令状态 FPS（预期：~200Hz）
212    pub master_slave_control_mode: f64,
213    /// 主从模式关节控制指令状态 FPS（预期：~500Hz）
214    pub master_slave_joint_control: f64,
215    /// 主从模式夹爪控制指令状态 FPS（预期：~200Hz）
216    pub master_slave_gripper_control: f64,
217}
218
219/// FPS 计数器值
220///
221/// 包含各状态的更新计数（原始值）。
222#[derive(Debug, Clone, Copy)]
223pub struct FpsCounts {
224    /// 关节位置状态更新次数
225    pub joint_position: u64,
226    /// 末端位姿状态更新次数
227    pub end_pose: u64,
228    /// 关节动态状态更新次数
229    pub joint_dynamic: u64,
230    /// 机器人控制状态更新次数
231    pub robot_control: u64,
232    /// 夹爪状态更新次数
233    pub gripper: u64,
234    /// 关节驱动器低速反馈状态更新次数
235    pub joint_driver_low_speed: u64,
236    /// 碰撞保护状态更新次数
237    pub collision_protection: u64,
238    /// 关节限制配置状态更新次数
239    pub joint_limit_config: u64,
240    /// 关节加速度限制配置状态更新次数
241    pub joint_accel_config: u64,
242    /// 末端限制配置状态更新次数
243    pub end_limit_config: u64,
244    /// 固件版本状态更新次数
245    pub firmware_version: u64,
246    /// 主从模式控制模式指令状态更新次数
247    pub master_slave_control_mode: u64,
248    /// 主从模式关节控制指令状态更新次数
249    pub master_slave_joint_control: u64,
250    /// 主从模式夹爪控制指令状态更新次数
251    pub master_slave_gripper_control: u64,
252}
253
254#[cfg(test)]
255mod tests {
256    use super::*;
257    use std::thread;
258    use std::time::Duration;
259
260    #[test]
261    fn test_fps_statistics_new() {
262        let stats = FpsStatistics::new();
263        let counts = stats.get_counts();
264
265        assert_eq!(counts.joint_dynamic, 0);
266        assert_eq!(counts.robot_control, 0);
267        assert_eq!(counts.gripper, 0);
268    }
269
270    #[test]
271    fn test_fps_statistics_reset() {
272        let mut stats = FpsStatistics::new();
273
274        // 模拟更新
275        stats.joint_position_updates.fetch_add(100, Ordering::Relaxed);
276        stats.end_pose_updates.fetch_add(50, Ordering::Relaxed);
277
278        // 验证计数不为 0
279        let counts_before = stats.get_counts();
280        assert!(counts_before.joint_position > 0);
281
282        // 重置
283        stats.reset();
284
285        // 验证重置后计数器为 0
286        let counts_after = stats.get_counts();
287        assert_eq!(counts_after.joint_position, 0);
288        assert_eq!(counts_after.end_pose, 0);
289    }
290
291    #[test]
292    fn test_fps_statistics_calculate_fps() {
293        let stats = FpsStatistics::new();
294
295        // 初始 FPS 应该为 0（没有更新）
296        let fps_initial = stats.calculate_fps();
297        assert_eq!(fps_initial.joint_position, 0.0);
298
299        // 模拟更新（500 次，模拟 500Hz 在 1 秒内的更新）
300        stats.joint_position_updates.fetch_add(500, Ordering::Relaxed);
301
302        // 使用精确的时间测量，而不是依赖 sleep 的准确性
303        let start = Instant::now();
304        thread::sleep(Duration::from_secs(1));
305        let actual_elapsed = start.elapsed();
306
307        // FPS 应该接近 500（允许一定误差）
308        // 在 CI 环境中，实际睡眠时间可能超过 1 秒，导致 FPS 偏低
309        // 使用实际经过的时间来计算期望值，并允许更大的容差
310        let expected_fps = 500.0;
311        let tolerance = if actual_elapsed.as_secs_f64() > 1.1 {
312            // 如果实际睡眠时间超过 1.1 秒，增加容差
313            100.0
314        } else {
315            50.0
316        };
317
318        let fps_after = stats.calculate_fps();
319        assert!(
320            (fps_after.joint_position - expected_fps).abs() < tolerance,
321            "Expected FPS ~{:.2}, got {:.2} (actual elapsed: {:.2}s)",
322            expected_fps,
323            fps_after.joint_position,
324            actual_elapsed.as_secs_f64()
325        );
326    }
327
328    #[test]
329    fn test_fps_statistics_get_counts() {
330        let stats = FpsStatistics::new();
331
332        // 模拟多次更新
333        stats.joint_position_updates.fetch_add(100, Ordering::Relaxed);
334        stats.end_pose_updates.fetch_add(150, Ordering::Relaxed);
335        stats.joint_dynamic_updates.fetch_add(200, Ordering::Relaxed);
336        stats.robot_control_updates.fetch_add(50, Ordering::Relaxed);
337
338        let counts = stats.get_counts();
339        assert_eq!(counts.joint_position, 100);
340        assert_eq!(counts.end_pose, 150);
341        assert_eq!(counts.joint_dynamic, 200);
342        assert_eq!(counts.robot_control, 50);
343        assert_eq!(counts.gripper, 0);
344    }
345
346    #[test]
347    fn test_fps_statistics_elapsed() {
348        let stats = FpsStatistics::new();
349
350        // 立即查询，经过时间应该很小
351        let elapsed = stats.elapsed();
352        assert!(elapsed.as_millis() < 100);
353
354        // 等待 100ms
355        thread::sleep(Duration::from_millis(100));
356
357        // 经过时间应该 >= 100ms
358        let elapsed_after = stats.elapsed();
359        assert!(elapsed_after.as_millis() >= 100);
360    }
361}
piper_driver/fps_stats.rs

piper_driver/
fps_stats.rs
