326 lines
12 KiB
Python
326 lines
12 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
无人系统控制器抽象基类
|
||
─────────────────────────────────────────────────────────────────
|
||
BaseUASController 所有无人系统控制器的统一抽象基类
|
||
|
||
职责：
|
||
- 持有 BaseProtocolAdapter 实例（可运行时热切换）
|
||
- 提供统一的高层控制 API（arm/takeoff/goto 等）
|
||
- 内部将高层 API 转换为 Command 并通过 Adapter 发送
|
||
- 维护设备连接状态和最新遥测缓存
|
||
- 支持指令队列与优先级调度
|
||
─────────────────────────────────────────────────────────────────
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import logging
|
||
import threading
|
||
import time
|
||
from abc import abstractmethod
|
||
from typing import Callable, Dict, List, Optional
|
||
|
||
from PyQt5.QtCore import QObject, QTimer, pyqtSignal
|
||
|
||
from uas.uas_control import command_registry
|
||
from uas.uas_control.base_adapter import BaseProtocolAdapter
|
||
from uas.uas_control.command import Command, CommandResult, CommandType, Commands
|
||
from uas.uas_control.telemetry import BaseTelemetry, ConnectionState
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 控制器抽象基类
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class BaseUASController(QObject):
|
||
"""
|
||
无人系统控制器抽象基类
|
||
|
||
── 子类必须实现 ──────────────────────────────────────────────
|
||
device_type 属性：设备类型字符串
|
||
_build_telemetry_type() 返回对应的遥测类型
|
||
|
||
── 子类可选覆盖 ──────────────────────────────────────────────
|
||
_on_telemetry(telem) 遥测数据到达回调
|
||
_on_connected() 连接成功回调
|
||
_on_disconnected() 断开连接回调
|
||
_pre_send(cmd) 指令发送前拦截（返回 False 则取消）
|
||
_post_send(cmd, result) 指令发送后回调
|
||
"""
|
||
|
||
# ── PyQt5 信号 ────────────────────────────────────────────────
|
||
telemetry_updated = pyqtSignal(object) # BaseTelemetry
|
||
command_sent = pyqtSignal(object) # Command
|
||
command_result = pyqtSignal(object) # CommandResult
|
||
connection_changed = pyqtSignal(str, str) # (device_id, state)
|
||
error_occurred = pyqtSignal(str) # 错误消息
|
||
|
||
# 遥测轮询间隔（毫秒）
|
||
TELEMETRY_INTERVAL_MS: int = 100
|
||
|
||
def __init__(
|
||
self,
|
||
device_id: str,
|
||
adapter: BaseProtocolAdapter,
|
||
parent: QObject = None,
|
||
):
|
||
super().__init__(parent)
|
||
self.device_id = device_id
|
||
self._adapter: BaseProtocolAdapter = adapter
|
||
self._telemetry: Optional[BaseTelemetry] = None
|
||
self._cmd_history: List[CommandResult] = []
|
||
self._callbacks: Dict[CommandType, Callable] = {}
|
||
self._lock = threading.Lock()
|
||
|
||
self.logger = logging.getLogger(
|
||
f"{self.__class__.__name__}[{device_id}]"
|
||
)
|
||
|
||
# 连接 Adapter 信号
|
||
self._adapter.connected.connect(self._on_adapter_connected)
|
||
self._adapter.disconnected.connect(self._on_adapter_disconnected)
|
||
self._adapter.telemetry_ready.connect(self._on_telemetry_signal)
|
||
self._adapter.command_ack.connect(self._on_command_ack)
|
||
self._adapter.error_occurred.connect(self._on_adapter_error)
|
||
|
||
# 遥测轮询定时器
|
||
self._telem_timer = QTimer(self)
|
||
self._telem_timer.timeout.connect(self._poll_telemetry)
|
||
|
||
# ── 连接管理 ──────────────────────────────────────────────────
|
||
|
||
def connect(self) -> bool:
|
||
"""连接设备"""
|
||
ok = self._adapter.connect()
|
||
if ok:
|
||
self._telem_timer.start(self.TELEMETRY_INTERVAL_MS)
|
||
return ok
|
||
|
||
def disconnect(self) -> bool:
|
||
"""断开设备"""
|
||
self._telem_timer.stop()
|
||
return self._adapter.disconnect()
|
||
|
||
def reconnect(self) -> bool:
|
||
"""重连设备"""
|
||
self._telem_timer.stop()
|
||
ok = self._adapter.reconnect()
|
||
if ok:
|
||
self._telem_timer.start(self.TELEMETRY_INTERVAL_MS)
|
||
return ok
|
||
|
||
def switch_adapter(self, new_adapter: BaseProtocolAdapter) -> None:
|
||
"""
|
||
热切换协议适配器（运行时切换 MAVLink ↔ ROS 等）
|
||
切换前自动断开旧适配器，切换后重新连接。
|
||
"""
|
||
was_connected = self._adapter.is_connected
|
||
self._telem_timer.stop()
|
||
|
||
# 断开旧适配器信号
|
||
try:
|
||
self._adapter.connected.disconnect()
|
||
self._adapter.disconnected.disconnect()
|
||
self._adapter.telemetry_ready.disconnect()
|
||
self._adapter.command_ack.disconnect()
|
||
self._adapter.error_occurred.disconnect()
|
||
except Exception:
|
||
pass
|
||
self._adapter.disconnect()
|
||
|
||
# 切换
|
||
self._adapter = new_adapter
|
||
self._adapter.connected.connect(self._on_adapter_connected)
|
||
self._adapter.disconnected.connect(self._on_adapter_disconnected)
|
||
self._adapter.telemetry_ready.connect(self._on_telemetry_signal)
|
||
self._adapter.command_ack.connect(self._on_command_ack)
|
||
self._adapter.error_occurred.connect(self._on_adapter_error)
|
||
|
||
self.logger.info(
|
||
f"协议适配器已切换: {new_adapter.__class__.__name__}"
|
||
)
|
||
|
||
if was_connected:
|
||
self.connect()
|
||
|
||
# ── 指令发送（核心）──────────────────────────────────────────
|
||
def send(self, cmd: Command) -> CommandResult:
|
||
"""
|
||
发送控制指令（统一入口）
|
||
|
||
流程：
|
||
① _pre_send() 前置拦截
|
||
② 检查连接状态
|
||
③ 通过 Adapter 发送
|
||
④ 记录历史 + 发射信号
|
||
⑤ _post_send() 后置回调
|
||
"""
|
||
# ① 前置拦截
|
||
if not self._pre_send(cmd):
|
||
result = CommandResult.fail(cmd, "前置检查未通过")
|
||
self.command_result.emit(result)
|
||
return result
|
||
|
||
# ② 连接检查
|
||
if not self._adapter.is_connected:
|
||
result = CommandResult.fail(cmd, "设备未连接")
|
||
self.command_result.emit(result)
|
||
return result
|
||
|
||
# ③ 发送
|
||
t0 = time.time()
|
||
result = self._adapter.send_command(cmd)
|
||
result.elapsed_ms = (time.time() - t0) * 1000
|
||
|
||
# ④ 记录 + 信号
|
||
with self._lock:
|
||
self._cmd_history.append(result)
|
||
if len(self._cmd_history) > 500:
|
||
self._cmd_history = self._cmd_history[-500:]
|
||
|
||
self.command_sent.emit(cmd)
|
||
self.command_result.emit(result)
|
||
self.logger.debug(f"指令结果: {result}")
|
||
|
||
# ⑤ 后置回调
|
||
self._post_send(cmd, result)
|
||
|
||
# 自定义回调
|
||
cb = self._callbacks.get(cmd.command_type)
|
||
if cb:
|
||
cb(result)
|
||
|
||
return result
|
||
|
||
# ── 通用高层 API ──────────────────────────────────────────────
|
||
@command_registry.register
|
||
def arm(self) -> CommandResult:
|
||
return self.send(Commands.arm(self.device_id))
|
||
|
||
@command_registry.register
|
||
def disarm(self) -> CommandResult:
|
||
return self.send(Commands.disarm(self.device_id))
|
||
|
||
@command_registry.register
|
||
def emergency_stop(self) -> CommandResult:
|
||
return self.send(Commands.emergency_stop(self.device_id))
|
||
|
||
@command_registry.register
|
||
def set_mode(self, mode: str) -> CommandResult:
|
||
return self.send(Commands.set_mode(self.device_id, mode))
|
||
|
||
@command_registry.register
|
||
def goto(self, lat: float, lon: float,
|
||
alt: float, speed: float = 5.0) -> CommandResult:
|
||
return self.send(Commands.goto(
|
||
self.device_id, lat, lon, alt, speed
|
||
))
|
||
|
||
@command_registry.register
|
||
def set_velocity(self, vx: float, vy: float,
|
||
vz: float = 0.0) -> CommandResult:
|
||
return self.send(Commands.set_velocity(
|
||
self.device_id, vx, vy, vz
|
||
))
|
||
|
||
def custom_command(self, model: int, **params) -> CommandResult:
|
||
return self.send(Commands.custom(self.device_id, model, **params))
|
||
|
||
# ── 遥测访问 ──────────────────────────────────────────────────
|
||
|
||
def get_telemetry(self) -> Optional[BaseTelemetry]:
|
||
# if self._telemetry is None:
|
||
# self._telemetry = self._adapter.get_telemetry()
|
||
# return self._telemetry
|
||
self._poll_telemetry()
|
||
return self._telemetry
|
||
|
||
@property
|
||
def is_connected(self) -> bool:
|
||
return self._adapter.is_connected
|
||
|
||
@property
|
||
def command_history(self) -> List[CommandResult]:
|
||
with self._lock:
|
||
return list(self._cmd_history)
|
||
|
||
@property
|
||
def adapter(self):
|
||
return self._adapter
|
||
|
||
# ── 回调注册 ──────────────────────────────────────────────────
|
||
|
||
def register_callback(
|
||
self,
|
||
cmd_type: CommandType,
|
||
callback: Callable[[CommandResult], None],
|
||
) -> None:
|
||
"""注册指令完成回调"""
|
||
self._callbacks[cmd_type] = callback
|
||
|
||
def unregister_callback(self, cmd_type: CommandType) -> None:
|
||
self._callbacks.pop(cmd_type, None)
|
||
|
||
# ── 抽象属性 ──────────────────────────────────────────────────
|
||
|
||
@property
|
||
@abstractmethod
|
||
def device_type(self) -> str:
|
||
"""设备类型字符串（如 'controller' / 'robot_dog'）"""
|
||
...
|
||
|
||
# ── 可选覆盖钩子 ──────────────────────────────────────────────
|
||
|
||
def _on_telemetry(self, telem: BaseTelemetry) -> None:
|
||
pass
|
||
|
||
def _on_connected(self) -> None:
|
||
pass
|
||
|
||
def _on_disconnected(self) -> None:
|
||
pass
|
||
|
||
def _pre_send(self, cmd: Command) -> bool:
|
||
return True
|
||
|
||
def _post_send(self, cmd: Command, result: CommandResult) -> None:
|
||
pass
|
||
|
||
# ── 内部槽 ───────────────────────────────────────────────────
|
||
def _poll_telemetry(self) -> None:
|
||
telem = self._adapter.get_telemetry()
|
||
if telem:
|
||
self._telemetry = telem
|
||
self.telemetry_updated.emit(telem)
|
||
self._on_telemetry(telem)
|
||
|
||
def _on_adapter_connected(self, device_id: str) -> None:
|
||
self.connection_changed.emit(device_id,
|
||
ConnectionState.CONNECTED.value)
|
||
self._on_connected()
|
||
|
||
def _on_adapter_disconnected(self, device_id: str) -> None:
|
||
self.connection_changed.emit(device_id,
|
||
ConnectionState.DISCONNECTED.value)
|
||
self._on_disconnected()
|
||
|
||
def _on_telemetry_signal(self, telem: BaseTelemetry) -> None:
|
||
self._telemetry = telem
|
||
self.telemetry_updated.emit(telem)
|
||
self._on_telemetry(telem)
|
||
|
||
def _on_command_ack(self, result: CommandResult) -> None:
|
||
self.command_result.emit(result)
|
||
|
||
def _on_adapter_error(self, msg: str) -> None:
|
||
self.error_occurred.emit(msg)
|
||
self.logger.error(f"Adapter 错误: {msg}")
|
||
|
||
def __repr__(self) -> str:
|
||
state = "已连接" if self.is_connected else "未连接"
|
||
return (f"{self.__class__.__name__}("
|
||
f"id={self.device_id}, "
|
||
f"type={self.device_type}, "
|
||
f"adapter={self._adapter.__class__.__name__}, "
|
||
f"{state})") |