mecha10_core/
teleop.rs

1//! Teleoperation message types
2//!
3//! This module contains message types for teleoperation control from multiple sources
4//! (CLI, dashboard, game controllers, etc.)
5
6use crate::messages::Message;
7use serde::{Deserialize, Serialize};
8
9// ============================================================================
10// Teleop Input
11// ============================================================================
12
13/// Input command from any teleoperation source
14///
15/// Multiple sources can send commands, and the teleop node will arbitrate
16/// between them based on priority and timestamps.
17#[derive(Debug, Clone, Serialize, Deserialize)]
18pub struct TeleopInput {
19    /// Source identifier (e.g., "cli", "dashboard", "gamepad")
20    pub source: String,
21
22    /// Priority level (0-255, higher = more important)
23    /// Typical values:
24    /// - 255: Emergency stop
25    /// - 200: Safety operator override
26    /// - 100: Primary operator (gamepad)
27    /// - 50: Secondary operator (dashboard)
28    /// - 10: Autonomous fallback
29    pub priority: u8,
30
31    /// Linear velocity command in m/s
32    pub linear: f32,
33
34    /// Angular velocity command in rad/s
35    pub angular: f32,
36
37    /// Timestamp in microseconds (from now_micros)
38    pub timestamp: u64,
39
40    /// Emergency stop flag - immediately halt all motion
41    pub emergency_stop: bool,
42}
43
44impl Message for TeleopInput {}
45
46impl TeleopInput {
47    /// Create a new teleop input with standard priority
48    pub fn new(source: impl Into<String>, linear: f32, angular: f32, timestamp: u64) -> Self {
49        Self {
50            source: source.into(),
51            priority: 50, // Default priority
52            linear,
53            angular,
54            timestamp,
55            emergency_stop: false,
56        }
57    }
58
59    /// Create an emergency stop command
60    pub fn emergency_stop(source: impl Into<String>, timestamp: u64) -> Self {
61        Self {
62            source: source.into(),
63            priority: 255,
64            linear: 0.0,
65            angular: 0.0,
66            timestamp,
67            emergency_stop: true,
68        }
69    }
70
71    /// Set the priority level
72    pub fn with_priority(mut self, priority: u8) -> Self {
73        self.priority = priority;
74        self
75    }
76
77    /// Check if command is expired (older than timeout_ms milliseconds)
78    pub fn is_expired(&self, current_time: u64, timeout_ms: u64) -> bool {
79        let age_us = current_time.saturating_sub(self.timestamp);
80        let age_ms = age_us / 1000;
81        age_ms > timeout_ms
82    }
83
84    /// Get age in milliseconds
85    pub fn age_ms(&self, current_time: u64) -> u64 {
86        let age_us = current_time.saturating_sub(self.timestamp);
87        age_us / 1000
88    }
89}
90
91// ============================================================================
92// Teleop Status
93// ============================================================================
94
95/// Status information from the teleop node
96///
97/// Published periodically so UIs can display current state
98#[derive(Debug, Clone, Serialize, Deserialize)]
99pub struct TeleopStatus {
100    /// Currently active control source
101    pub active_source: String,
102
103    /// Age of last command in milliseconds
104    pub last_command_age_ms: u64,
105
106    /// Safety system engaged (command timeout or e-stop)
107    pub safety_engaged: bool,
108
109    /// Current linear velocity being sent to motors (m/s)
110    pub current_linear: f32,
111
112    /// Current angular velocity being sent to motors (rad/s)
113    pub current_angular: f32,
114
115    /// Number of commands received since startup
116    pub total_commands: u64,
117
118    /// Priority of active source
119    pub active_priority: u8,
120}
121
122impl Message for TeleopStatus {}
123
124impl TeleopStatus {
125    /// Create a new status with default values
126    pub fn new() -> Self {
127        Self {
128            active_source: "none".to_string(),
129            last_command_age_ms: 0,
130            safety_engaged: false,
131            current_linear: 0.0,
132            current_angular: 0.0,
133            total_commands: 0,
134            active_priority: 0,
135        }
136    }
137
138    /// Check if teleop is actively controlling
139    pub fn is_active(&self) -> bool {
140        !self.safety_engaged && self.active_source != "none"
141    }
142}
143
144impl Default for TeleopStatus {
145    fn default() -> Self {
146        Self::new()
147    }
148}
mecha10_core/teleop.rs

mecha10_core/
teleop.rs
