wenchun
/
AirTrapMine
forked from chiyu1468/AirTrapMine
1
0
Fork 0
Code Pull Requests Activity

add vehicleDiagShow.py for vehicle diagnostics

Browse Source 
- Introduced vehicleDiagShow.py, 這個簡易的終端機介面 顯示載具的自檢驗與狀態
Chiyu Chen 2 weeks ago
parent bc841156cb
commit ab616ceb54
3 changed files with 546 additions and 193 deletions
Show Stats Download Patch File Download Diff File

2
README.md
Unescape Escape View File

@ -67,6 +67,8 @@ cd ~/AirTrapMine/src/ # 這是範例!!!
python -m fc_network_adapter.fc_network_adapter.mainOrchestrator python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
python -m fc_network_adapter.tests.demo_integration python -m fc_network_adapter.tests.demo_integration
python -m someotherpkg.src.example_wholeMoving python -m someotherpkg.src.example_wholeMoving
python fc_network_apps/vehicleDiagShow.py
``` ```
2. 2.

193
serial_udp_update.py
Unescape Escape View File

@ -1,193 +0,0 @@
import asyncio
import serial_asyncio
import struct
import os
import sys
import serial
import signal
import traceback
from pymavlink import mavutil
# === 設定區 ===
SERIAL_PORT = 'COM15' # 手動指定
SERIAL_BAUDRATE = 57600
UDP_REMOTE_IP = '127.0.0.1'
UDP_REMOTE_PORT = 14550
DEBUG_MODE = False
TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播
# === 工具函數 ===
def check_serial_port():
try:
ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
ser.close()
return True
except serial.SerialException as e:
print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
return False
except Exception as e:
print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
return False
def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
frame_type = 0x10
dest_addr16 = b'\xFF\xFE'
broadcast_radius = 0x00
options = 0x00
frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
frame += dest_addr64 + dest_addr16
frame += struct.pack(">BB", broadcast_radius, options) + data
checksum = 0xFF - (sum(frame) & 0xFF)
return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
# === Serial Protocol 實作 ===
class SerialToUDP(asyncio.Protocol):
def __init__(self, udp_protocol):
self.udp_protocol = udp_protocol
self.buffer = bytearray()
def connection_made(self, transport):
self.transport = transport
if hasattr(self.udp_protocol, 'set_serial_transport'):
self.udp_protocol.set_serial_transport(self)
print(f"Serial connection established on {SERIAL_PORT}")
def data_received(self, data):
self.buffer.extend(data)
while True:
if len(self.buffer) < 3:
return
if self.buffer[0] != 0x7E:
self.buffer.pop(0)
continue
length = (self.buffer[1] << 8) | self.buffer[2]
full_length = 3 + length + 1
if len(self.buffer) < full_length:
return
frame = self.buffer[:full_length]
del self.buffer[:full_length]
if hasattr(self.udp_protocol, 'send_udp'):
self.udp_protocol.send_udp(bytes(frame))
def write_to_serial(self, data):
try:
api_frame = build_api_tx_frame(data, TARGET_ADDR64)
pass
self.transport.write(api_frame)
except Exception as e:
print(f"[TX Error] 無法封裝或傳送資料: {e}")
# === UDP Protocol 實作 ===
class UDPHandler(asyncio.DatagramProtocol):
def __init__(self):
self.serial_transport = None
self.transport = None
self.mav_decoder = mavutil.mavlink.MAVLink(None)
def connection_made(self, transport):
self.transport = transport
print("UDP transport ready.")
def set_serial_transport(self, serial_transport):
self.serial_transport = serial_transport
def datagram_received(self, data, addr):
if self.serial_transport:
self.serial_transport.write_to_serial(data)
def send_udp(self, data):
decoded_data = self.decapsulate_data(data)
if decoded_data is None:
pass
return
self.decode_mavlink_data(decoded_data)
if self.transport:
self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
def decapsulate_data(self, data):
try:
if not data or data[0] != 0x7E:
return None
length = (data[1] << 8) | data[2]
if len(data) < length + 4:
return None
frame_type = data[3]
if frame_type == 0x90:
rf_data_start = 3 + 12
return data[rf_data_start:3 + length]
else:
return None
except Exception as e:
print(f"[XBee 解封錯誤] {e}")
return None
def decode_mavlink_data(self, data):
try:
msg = self.mav_decoder.parse_char(data)
if msg:
if msg.get_type() == "HEARTBEAT":
pass # 不輸出任何訊息
else:
pass
except Exception as e:
print(f"[MAVLink Decode Error] {e}")
# === 主流程 ===
async def main():
if not check_serial_port():
print("程式終止：串口檢查失敗")
return
loop = asyncio.get_running_loop()
if os.name != 'nt': # Windows 不支援 add_signal_handler
for sig in (signal.SIGINT, signal.SIGTERM):
loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
udp_handler = UDPHandler()
try:
udp_transport, _ = await loop.create_datagram_endpoint(
lambda: udp_handler,
local_addr=('0.0.0.0', 0)
)
except Exception as e:
print(f"無法創建 UDP 端點：{str(e)}")
return
sock = udp_transport.get_extra_info('socket')
print(f"UDP listening on {sock.getsockname()}")
try:
serial_proto = SerialToUDP(udp_handler)
await serial_asyncio.create_serial_connection(
loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
)
except Exception as e:
print(f"無法建立串口連接：{str(e)}")
traceback.print_exc()
udp_transport.close()
return
print("等待串口資料...")
try:
await asyncio.Future()
except asyncio.CancelledError:
pass
async def shutdown(loop):
print("Shutting down...")
tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
for task in tasks:
task.cancel()
await asyncio.gather(*tasks, return_exceptions=True)
loop.stop()
if __name__ == '__main__':
try:
asyncio.run(main())
except KeyboardInterrupt:
print("程式被使用者中斷")
except Exception as e:
print("程式執行錯誤：")
traceback.print_exc()

544
src/fc_network_apps/vehicleDiagShow.py
Unescape Escape View File

@ -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())
Write Preview
Loading…
Cancel
Save