base_agent/uas/uas_control/base_controller.py

# -*- 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})")