add vehicleDiagShow.py for vehicle diagnostics
- Introduced vehicleDiagShow.py, 這個簡易的終端機介面 顯示載具的自檢驗與狀態
This commit is contained in:
@@ -0,0 +1,544 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Standalone curses TUI for fc_network vehicle diagnostics (sys_diags, status_text, summary)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import curses
|
||||
import json
|
||||
import re
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
|
||||
from fc_interfaces.msg import SystemDiagnosticsRaw
|
||||
|
||||
VEHICLE_TOPIC_RE = re.compile(
|
||||
r'^/fc_network/vehicle/(sys\d+)/(sys_diags|status_text|summary)$'
|
||||
)
|
||||
STATUS_TEXT_RE = re.compile(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$')
|
||||
|
||||
STATUS_TEXT_TTL_SEC = 30.0
|
||||
STALE_SEC = 3.0
|
||||
RESCAN_SEC = 2.0
|
||||
OFFLINE_SEC = 10.0
|
||||
DRAW_HZ = 5.0
|
||||
|
||||
# MAV_SYS_STATUS_SENSOR bit labels (MAVLink common)
|
||||
SENSOR_BITS: Tuple[Tuple[int, str], ...] = (
|
||||
(268435456, 'PreArm'),
|
||||
(1, 'Gyro'),
|
||||
(2, 'Accel'),
|
||||
(4, 'Mag'),
|
||||
(8, 'Baro'),
|
||||
(16, 'DiffPress'),
|
||||
(32, 'GPS'),
|
||||
(64, 'OptFlow'),
|
||||
(128, 'Vision'),
|
||||
(256, 'Laser'),
|
||||
(512, 'ExtGT'),
|
||||
(1024, 'AngRate'),
|
||||
(2048, 'AttStab'),
|
||||
(4096, 'YawPos'),
|
||||
(8192, 'AltCtrl'),
|
||||
(16384, 'XYCtrl'),
|
||||
(32768, 'Motor'),
|
||||
(65536, 'RC'),
|
||||
(131072, 'Gyro2'),
|
||||
(262144, 'Accel2'),
|
||||
(524288, 'Mag2'),
|
||||
(1048576, 'Geofence'),
|
||||
(2097152, 'AHRS'),
|
||||
(4194304, 'Terrain'),
|
||||
(8388608, 'RevMotor'),
|
||||
(16777216, 'Log'),
|
||||
(33554432, 'Batt'),
|
||||
(67108864, 'Prox'),
|
||||
(134217728, 'Satcom'),
|
||||
(536870912, 'ObsAvoid'),
|
||||
(1073741824, 'Propulsion'),
|
||||
)
|
||||
|
||||
MAV_STATE_LABELS = {
|
||||
0: 'UNINIT',
|
||||
1: 'BOOT',
|
||||
2: 'CAL',
|
||||
3: 'STANDBY',
|
||||
4: 'ACTIVE',
|
||||
5: 'CRITICAL',
|
||||
6: 'EMERGENCY',
|
||||
7: 'OFF',
|
||||
8: 'TERM',
|
||||
}
|
||||
|
||||
SEVERITY_LABELS = {
|
||||
0: 'EMERG',
|
||||
1: 'ALERT',
|
||||
2: 'CRIT',
|
||||
3: 'ERROR',
|
||||
4: 'WARN',
|
||||
5: 'NOTICE',
|
||||
6: 'INFO',
|
||||
7: 'DEBUG',
|
||||
}
|
||||
|
||||
|
||||
@dataclass
|
||||
class StatusTextEntry:
|
||||
received_at: float
|
||||
severity: int
|
||||
text: str
|
||||
raw: str
|
||||
sys_key: str
|
||||
|
||||
|
||||
@dataclass
|
||||
class VehicleDiagState:
|
||||
sys_key: str
|
||||
sysid: int
|
||||
summary: Optional[dict] = None
|
||||
diag: Optional[SystemDiagnosticsRaw] = None
|
||||
status_entries: List[StatusTextEntry] = field(default_factory=list)
|
||||
last_summary_ts: float = 0.0
|
||||
last_diag_ts: float = 0.0
|
||||
last_topic_seen_ts: float = 0.0
|
||||
offline: bool = False
|
||||
|
||||
|
||||
def sys_key_to_id(sys_key: str) -> int:
|
||||
return int(sys_key.replace('sys', '', 1))
|
||||
|
||||
|
||||
def purge_status_entries(entries: List[StatusTextEntry], now: float, ttl: float = STATUS_TEXT_TTL_SEC) -> None:
|
||||
cutoff = now - ttl
|
||||
entries[:] = [e for e in entries if e.received_at >= cutoff]
|
||||
|
||||
|
||||
def parse_status_text(raw: str, sys_key: str, received_at: float) -> StatusTextEntry:
|
||||
match = STATUS_TEXT_RE.match(raw.strip())
|
||||
if match:
|
||||
severity = int(match.group(2))
|
||||
text = match.group(3)
|
||||
else:
|
||||
severity = -1
|
||||
text = raw
|
||||
return StatusTextEntry(
|
||||
received_at=received_at,
|
||||
severity=severity,
|
||||
text=text,
|
||||
raw=raw,
|
||||
sys_key=sys_key,
|
||||
)
|
||||
|
||||
|
||||
def decode_sensors(install: int, enabled: int, health: int) -> List[Tuple[str, str]]:
|
||||
"""Return list of (label, status) where status is OK / FAIL / OFF."""
|
||||
items: List[Tuple[str, str]] = []
|
||||
for bit, label in SENSOR_BITS:
|
||||
if not (install & bit):
|
||||
continue
|
||||
if not (enabled & bit):
|
||||
items.append((label, 'OFF'))
|
||||
elif health & bit:
|
||||
items.append((label, 'OK'))
|
||||
else:
|
||||
items.append((label, 'FAIL'))
|
||||
return items
|
||||
|
||||
|
||||
def format_age(seconds: float) -> str:
|
||||
if seconds < 0:
|
||||
return 'never'
|
||||
if seconds < 1.0:
|
||||
return f'{seconds * 1000:.0f}ms'
|
||||
if seconds < 60.0:
|
||||
return f'{seconds:.1f}s'
|
||||
return f'{seconds / 60:.1f}m'
|
||||
|
||||
|
||||
def truncate(text: str, width: int) -> str:
|
||||
if width <= 0:
|
||||
return ''
|
||||
if len(text) <= width:
|
||||
return text
|
||||
if width <= 3:
|
||||
return text[:width]
|
||||
return text[: width - 3] + '...'
|
||||
|
||||
|
||||
class VehicleDiagNode(Node):
|
||||
"""ROS2 node: discover vehicles and subscribe to diagnostic topics."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__('vehicle_diag_show')
|
||||
self._lock = threading.Lock()
|
||||
self._vehicles: Dict[str, VehicleDiagState] = {}
|
||||
self._subs: Dict[str, Dict[str, object]] = {}
|
||||
self._scan_timer = self.create_timer(RESCAN_SEC, self._rescan_topics)
|
||||
self._rescan_topics()
|
||||
|
||||
def snapshot(self) -> Dict[str, VehicleDiagState]:
|
||||
now = time.monotonic()
|
||||
with self._lock:
|
||||
for state in self._vehicles.values():
|
||||
purge_status_entries(state.status_entries, now)
|
||||
if state.last_topic_seen_ts > 0:
|
||||
state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
|
||||
return {k: state for k, state in self._vehicles.items()}
|
||||
|
||||
def _get_or_create_state(self, sys_key: str) -> VehicleDiagState:
|
||||
if sys_key not in self._vehicles:
|
||||
self._vehicles[sys_key] = VehicleDiagState(
|
||||
sys_key=sys_key,
|
||||
sysid=sys_key_to_id(sys_key),
|
||||
)
|
||||
return self._vehicles[sys_key]
|
||||
|
||||
def _rescan_topics(self) -> None:
|
||||
now = time.monotonic()
|
||||
found: set = set()
|
||||
for topic_name, _ in self.get_topic_names_and_types():
|
||||
match = VEHICLE_TOPIC_RE.match(topic_name)
|
||||
if not match:
|
||||
continue
|
||||
sys_key = match.group(1)
|
||||
found.add(sys_key)
|
||||
with self._lock:
|
||||
state = self._get_or_create_state(sys_key)
|
||||
state.last_topic_seen_ts = now
|
||||
state.offline = False
|
||||
self._ensure_subscriptions(sys_key)
|
||||
|
||||
with self._lock:
|
||||
for sys_key, state in self._vehicles.items():
|
||||
if sys_key not in found and state.last_topic_seen_ts > 0:
|
||||
state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
|
||||
|
||||
def _ensure_subscriptions(self, sys_key: str) -> None:
|
||||
if sys_key in self._subs:
|
||||
return
|
||||
base = f'/fc_network/vehicle/{sys_key}'
|
||||
subs = {
|
||||
'summary': self.create_subscription(
|
||||
String,
|
||||
f'{base}/summary',
|
||||
lambda msg, sk=sys_key: self._on_summary(sk, msg),
|
||||
10,
|
||||
),
|
||||
'sys_diags': self.create_subscription(
|
||||
SystemDiagnosticsRaw,
|
||||
f'{base}/sys_diags',
|
||||
lambda msg, sk=sys_key: self._on_sys_diags(sk, msg),
|
||||
10,
|
||||
),
|
||||
'status_text': self.create_subscription(
|
||||
String,
|
||||
f'{base}/status_text',
|
||||
lambda msg, sk=sys_key: self._on_status_text(sk, msg),
|
||||
10,
|
||||
),
|
||||
}
|
||||
with self._lock:
|
||||
self._subs[sys_key] = subs
|
||||
|
||||
def _on_summary(self, sys_key: str, msg: String) -> None:
|
||||
now = time.monotonic()
|
||||
try:
|
||||
summary = json.loads(msg.data)
|
||||
except json.JSONDecodeError:
|
||||
summary = {'raw': msg.data}
|
||||
with self._lock:
|
||||
state = self._get_or_create_state(sys_key)
|
||||
state.summary = summary
|
||||
state.last_summary_ts = now
|
||||
state.last_topic_seen_ts = now
|
||||
|
||||
def _on_sys_diags(self, sys_key: str, msg: SystemDiagnosticsRaw) -> None:
|
||||
now = time.monotonic()
|
||||
with self._lock:
|
||||
state = self._get_or_create_state(sys_key)
|
||||
state.diag = msg
|
||||
state.last_diag_ts = now
|
||||
state.last_topic_seen_ts = now
|
||||
|
||||
def _on_status_text(self, sys_key: str, msg: String) -> None:
|
||||
now = time.monotonic()
|
||||
entry = parse_status_text(msg.data, sys_key, now)
|
||||
with self._lock:
|
||||
state = self._get_or_create_state(sys_key)
|
||||
state.status_entries.append(entry)
|
||||
purge_status_entries(state.status_entries, now)
|
||||
state.last_topic_seen_ts = now
|
||||
|
||||
|
||||
class DiagCursesApp:
|
||||
"""Curses front-end for vehicle diagnostics."""
|
||||
|
||||
def __init__(self, node: VehicleDiagNode) -> None:
|
||||
self._node = node
|
||||
self._running = True
|
||||
|
||||
def run(self) -> None:
|
||||
curses.wrapper(self._main)
|
||||
|
||||
def _init_colors(self) -> None:
|
||||
curses.start_color()
|
||||
curses.use_default_colors()
|
||||
curses.init_pair(1, curses.COLOR_GREEN, -1) # OK / DISARMED / INFO
|
||||
curses.init_pair(2, curses.COLOR_RED, -1) # FAIL / ARMED / ERROR+
|
||||
curses.init_pair(3, curses.COLOR_YELLOW, -1) # WARN / OFF
|
||||
curses.init_pair(4, curses.COLOR_CYAN, -1) # header / INFO
|
||||
curses.init_pair(5, curses.COLOR_WHITE, -1) # dim / offline
|
||||
curses.init_pair(6, curses.COLOR_MAGENTA, -1) # NOTICE
|
||||
|
||||
def _main(self, stdscr) -> None:
|
||||
curses.curs_set(0)
|
||||
stdscr.nodelay(True)
|
||||
stdscr.keypad(True)
|
||||
self._init_colors()
|
||||
|
||||
while self._running and rclpy.ok():
|
||||
try:
|
||||
self._draw(stdscr)
|
||||
except curses.error:
|
||||
pass
|
||||
key = stdscr.getch()
|
||||
if key in (ord('q'), ord('Q'), 27):
|
||||
self._running = False
|
||||
time.sleep(1.0 / DRAW_HZ)
|
||||
|
||||
def _draw(self, stdscr) -> None:
|
||||
stdscr.erase()
|
||||
height, width = stdscr.getmaxyx()
|
||||
now = time.monotonic()
|
||||
vehicles = self._node.snapshot()
|
||||
sorted_keys = sorted(vehicles.keys(), key=sys_key_to_id)
|
||||
|
||||
header = f' Vehicle Diagnostics Monitor vehicles:{len(sorted_keys)} Q:quit '
|
||||
stdscr.attron(curses.color_pair(4) | curses.A_BOLD)
|
||||
stdscr.addstr(0, 0, truncate(header.ljust(width - 1), width - 1))
|
||||
stdscr.attroff(curses.color_pair(4) | curses.A_BOLD)
|
||||
|
||||
row = 1
|
||||
if not sorted_keys:
|
||||
msg = ' Waiting for vehicles... (need /fc_network/vehicle/sysN/* topics) '
|
||||
if row < height - 1:
|
||||
stdscr.addstr(row, max(0, (width - len(msg)) // 2), truncate(msg, width - 1))
|
||||
row += 2
|
||||
|
||||
vehicle_rows_end = max(row, height - 8)
|
||||
for sys_key in sorted_keys:
|
||||
if row >= vehicle_rows_end:
|
||||
break
|
||||
state = vehicles[sys_key]
|
||||
row = self._draw_vehicle_block(stdscr, row, width, state, now)
|
||||
if row < height - 1:
|
||||
try:
|
||||
stdscr.addstr(row, 0, truncate('-' * (width - 1), width - 1))
|
||||
except curses.error:
|
||||
pass
|
||||
row += 1
|
||||
|
||||
log_title = ' Status Text (last 30s) '
|
||||
if row < height - 1:
|
||||
stdscr.attron(curses.color_pair(4))
|
||||
stdscr.addstr(row, 0, truncate(log_title.ljust(width - 1), width - 1))
|
||||
stdscr.attroff(curses.color_pair(4))
|
||||
row += 1
|
||||
|
||||
all_entries: List[StatusTextEntry] = []
|
||||
for sys_key in sorted_keys:
|
||||
all_entries.extend(vehicles[sys_key].status_entries)
|
||||
all_entries.sort(key=lambda e: e.received_at)
|
||||
|
||||
log_rows = height - row - 1
|
||||
if log_rows <= 0:
|
||||
stdscr.refresh()
|
||||
return
|
||||
|
||||
if not all_entries:
|
||||
empty = ' (no messages in last 30s) '
|
||||
if row < height - 1:
|
||||
stdscr.addstr(row, 0, truncate(empty, width - 1))
|
||||
else:
|
||||
visible = all_entries[-log_rows:]
|
||||
for entry in visible:
|
||||
if row >= height - 1:
|
||||
break
|
||||
self._draw_status_line(stdscr, row, width, entry)
|
||||
row += 1
|
||||
|
||||
stdscr.refresh()
|
||||
|
||||
def _draw_vehicle_block(
|
||||
self,
|
||||
stdscr,
|
||||
row: int,
|
||||
width: int,
|
||||
state: VehicleDiagState,
|
||||
now: float,
|
||||
) -> int:
|
||||
summary = state.summary or {}
|
||||
mode = summary.get('mode_name', 'UNKNOWN')
|
||||
armed = summary.get('armed', False)
|
||||
conn = summary.get('connection_type', '?')
|
||||
socket_id = summary.get('socket_id', -1)
|
||||
mav_status = summary.get('mav_status', 0)
|
||||
mav_label = MAV_STATE_LABELS.get(int(mav_status), str(mav_status))
|
||||
|
||||
last_update = max(state.last_summary_ts, state.last_diag_ts)
|
||||
age_sec = (now - last_update) if last_update > 0 else -1.0
|
||||
stale = age_sec >= 0 and age_sec > STALE_SEC
|
||||
|
||||
armed_str = 'ARMED' if armed else 'DISARMED'
|
||||
flags = []
|
||||
if state.offline:
|
||||
flags.append('OFFLINE')
|
||||
if stale and not state.offline:
|
||||
flags.append('STALE')
|
||||
flag_str = f" [{' '.join(flags)}]" if flags else ''
|
||||
|
||||
line1 = (
|
||||
f' {state.sys_key} (MAV {state.sysid}) {mode} {armed_str} '
|
||||
f'{conn} socket:{socket_id} {mav_label} '
|
||||
f'updated {format_age(age_sec)}{flag_str}'
|
||||
)
|
||||
if row < stdscr.getmaxyx()[0] - 1:
|
||||
if state.offline:
|
||||
stdscr.attron(curses.color_pair(5))
|
||||
stdscr.addstr(row, 0, truncate(line1, width - 1))
|
||||
stdscr.attroff(curses.color_pair(5))
|
||||
else:
|
||||
stdscr.addstr(row, 0, truncate(line1, width - 1))
|
||||
armed_pos = line1.find(armed_str)
|
||||
if armed_pos >= 0:
|
||||
pair = curses.color_pair(2) if armed else curses.color_pair(1)
|
||||
stdscr.attron(pair | curses.A_BOLD)
|
||||
stdscr.addstr(row, armed_pos, armed_str)
|
||||
stdscr.attroff(pair | curses.A_BOLD)
|
||||
row += 1
|
||||
|
||||
if state.diag is not None and row < stdscr.getmaxyx()[0] - 1:
|
||||
load_pct = state.diag.mcu_load / 10.0
|
||||
drop_pct = state.diag.bus_error_rate / 10.0
|
||||
line2 = f' MCU {load_pct:.1f}% CommDrop {drop_pct:.1f}%'
|
||||
stdscr.addstr(row, 0, truncate(line2, width - 1))
|
||||
row += 1
|
||||
|
||||
sensors = decode_sensors(
|
||||
state.diag.sensors_install_mask,
|
||||
state.diag.sensors_enabled_mask,
|
||||
state.diag.sensors_health_mask,
|
||||
)
|
||||
row = self._draw_sensor_line(stdscr, row, width, sensors)
|
||||
elif row < stdscr.getmaxyx()[0] - 1:
|
||||
stdscr.addstr(row, 0, truncate(' (no sys_diags yet)', width - 1))
|
||||
row += 1
|
||||
|
||||
return row
|
||||
|
||||
def _draw_sensor_line(
|
||||
self,
|
||||
stdscr,
|
||||
row: int,
|
||||
width: int,
|
||||
sensors: List[Tuple[str, str]],
|
||||
) -> int:
|
||||
max_row, _ = stdscr.getmaxyx()
|
||||
if not sensors:
|
||||
if row < max_row - 1:
|
||||
stdscr.addstr(row, 0, truncate(' Sensors: (none installed)', width - 1))
|
||||
return row + 1
|
||||
|
||||
lines: List[List[Tuple[str, str]]] = [[]]
|
||||
line_len = len(' Sensors: ')
|
||||
for label, status in sensors:
|
||||
token = f'{label}:{status} '
|
||||
if lines[-1] and line_len + len(token) >= width - 1:
|
||||
lines.append([])
|
||||
line_len = 2
|
||||
lines[-1].append((label, status))
|
||||
line_len += len(token)
|
||||
|
||||
for idx, line_items in enumerate(lines):
|
||||
if row >= max_row - 1:
|
||||
break
|
||||
prefix = ' Sensors: ' if idx == 0 else ' '
|
||||
col = 0
|
||||
stdscr.addstr(row, col, prefix)
|
||||
col += len(prefix)
|
||||
for label, status in line_items:
|
||||
token = f'{label}:{status} '
|
||||
if col + len(token) >= width - 1:
|
||||
stdscr.addstr(row, col, truncate('...', width - col - 1))
|
||||
return row + 1
|
||||
pair = curses.A_NORMAL
|
||||
if status == 'OK':
|
||||
pair = curses.color_pair(1)
|
||||
elif status == 'FAIL':
|
||||
pair = curses.color_pair(2) | curses.A_BOLD
|
||||
elif status == 'OFF':
|
||||
pair = curses.color_pair(3)
|
||||
stdscr.attron(pair)
|
||||
stdscr.addstr(row, col, token)
|
||||
stdscr.attroff(pair)
|
||||
col += len(token)
|
||||
row += 1
|
||||
return row
|
||||
|
||||
def _draw_status_line(
|
||||
self,
|
||||
stdscr,
|
||||
row: int,
|
||||
width: int,
|
||||
entry: StatusTextEntry,
|
||||
) -> None:
|
||||
sev_label = SEVERITY_LABELS.get(entry.severity, str(entry.severity))
|
||||
prefix = f'[{entry.sys_key:5}] [{entry.severity} {sev_label:5}] '
|
||||
body = truncate(entry.text, max(0, width - len(prefix) - 2))
|
||||
line = prefix + body
|
||||
|
||||
pair = curses.A_NORMAL
|
||||
if entry.severity <= 3:
|
||||
pair = curses.color_pair(2)
|
||||
elif entry.severity == 4:
|
||||
pair = curses.color_pair(3)
|
||||
elif entry.severity == 5:
|
||||
pair = curses.color_pair(6)
|
||||
elif entry.severity == 6:
|
||||
pair = curses.color_pair(1)
|
||||
elif entry.severity == 7:
|
||||
pair = curses.color_pair(5)
|
||||
|
||||
stdscr.attron(pair)
|
||||
stdscr.addstr(row, 0, truncate(line, width - 1))
|
||||
stdscr.attroff(pair)
|
||||
|
||||
|
||||
def main() -> int:
|
||||
if not sys.stdout.isatty():
|
||||
print('vehicleDiagShow requires an interactive terminal (TTY).', file=sys.stderr)
|
||||
return 1
|
||||
|
||||
rclpy.init()
|
||||
node = VehicleDiagNode()
|
||||
spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
|
||||
spin_thread.start()
|
||||
|
||||
try:
|
||||
DiagCursesApp(node).run()
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
spin_thread.join(timeout=2.0)
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
Reference in New Issue
Block a user