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
130
131
132
133
//! Teleoperation message types
//!
//! This module contains message types for teleoperation control from multiple sources
//! (CLI, dashboard, game controllers, etc.)
use crate::messages::Message;
use serde::{Deserialize, Serialize};
/// Input command from any teleoperation source
///
/// Multiple sources can send commands, and the teleop node will arbitrate
/// between them based on priority and timestamps.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct TeleopInput {
/// Source identifier (e.g., "cli", "dashboard", "gamepad")
pub source: String,
/// Priority level (0-255, higher = more important)
/// Typical values:
/// - 255: Emergency stop
/// - 200: Safety operator override
/// - 100: Primary operator (gamepad)
/// - 50: Secondary operator (dashboard)
/// - 10: Autonomous fallback
pub priority: u8,
/// Linear velocity command in m/s
pub linear: f32,
/// Angular velocity command in rad/s
pub angular: f32,
/// Timestamp in microseconds (from now_micros)
pub timestamp: u64,
/// Emergency stop flag - immediately halt all motion
pub emergency_stop: bool,
}
impl Message for TeleopInput {}
impl TeleopInput {
/// Create a new teleop input with standard priority
pub fn new(source: impl Into<String>, linear: f32, angular: f32, timestamp: u64) -> Self {
Self {
source: source.into(),
priority: 50,
linear,
angular,
timestamp,
emergency_stop: false,
}
}
/// Create an emergency stop command
pub fn emergency_stop(source: impl Into<String>, timestamp: u64) -> Self {
Self {
source: source.into(),
priority: 255,
linear: 0.0,
angular: 0.0,
timestamp,
emergency_stop: true,
}
}
/// Set the priority level
pub fn with_priority(mut self, priority: u8) -> Self {
self.priority = priority;
self
}
/// Get age in milliseconds
pub fn age_ms(&self, current_time: u64) -> u64 {
let age_us = current_time.saturating_sub(self.timestamp);
age_us / 1000
}
}
/// Status information from the teleop node
///
/// Published periodically so UIs can display current state
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct TeleopStatus {
/// Currently active control source
pub active_source: String,
/// Age of last command in milliseconds
pub last_command_age_ms: u64,
/// Safety system engaged (command timeout or e-stop)
pub safety_engaged: bool,
/// Current linear velocity being sent to motors (m/s)
pub current_linear: f32,
/// Current angular velocity being sent to motors (rad/s)
pub current_angular: f32,
/// Number of commands received since startup
pub total_commands: u64,
/// Priority of active source
pub active_priority: u8,
}
impl Message for TeleopStatus {}
impl TeleopStatus {
/// Create a new status with default values
pub fn new() -> Self {
Self {
active_source: "none".to_string(),
last_command_age_ms: 0,
safety_engaged: false,
current_linear: 0.0,
current_angular: 0.0,
total_commands: 0,
active_priority: 0,
}
}
/// Check if teleop is actively controlling
pub fn is_active(&self) -> bool {
!self.safety_engaged && self.active_source != "none"
}
}
impl Default for TeleopStatus {
fn default() -> Self {
Self::new()
}
}