base_agent/uas/uas_control/protocols/simulation_adapter.py

from __future__ import annotations

import math
import random
import time
from typing import Any, Dict, List

from uas.uas_control.base_adapter import BaseProtocolAdapter, ProtocolType
from uas.uas_control.command import Command, CommandResult, CommandType
from uas.uas_control.telemetry import (
    BaseTelemetry, UAVTelemetry, RobotDogTelemetry, ConnectionState
)


# ─────────────────────────────────────────────────────────────────
# 仿真适配器（无需真实硬件，用于测试）
# ─────────────────────────────────────────────────────────────────

class SimulationAdapter(BaseProtocolAdapter):
    """
    仿真协议适配器

    无需任何外部依赖，完全在内存中模拟设备行为。
    支持所有 CommandType，适合单元测试和集成演示。

    config 字段：
      device_type   : "controller" | "robot_dog"
      fail_rate     : 指令随机失败概率（0.0~1.0，默认 0.0）
      latency_ms    : 模拟通信延迟（ms，默认 20）
    """

    _ALL_COMMANDS = list(CommandType)

    def __init__(
        self,
        device_id:   str,
        device_type: str   = "controller",
        fail_rate:   float = 0.0,
        latency_ms:  float = 20.0,
        parent             = None,
    ):
        config = dict(
            device_type = device_type,
            fail_rate   = fail_rate,
            latency_ms  = latency_ms,
        )
        super().__init__(device_id, ProtocolType.SIMULATION, config, parent)
        self._state: Dict[str, Any] = {
            "armed":    False,
            "mode":     "STABILIZE",
            "lat":      39.9,
            "lon":      116.4,
            "alt":      0.0,
            "heading":  0.0,
            "speed":    0.0,
            "battery":  100,
            "gait":     "STAND",
        }

    def connect(self) -> bool:
        self.logger.info(
            f"[仿真] 设备连接: {self.device_id} "
            f"type={self.config['device_type']}"
        )
        self._set_connected(True)
        return True

    def disconnect(self) -> bool:
        self._set_connected(False)
        return True

    def send_command(self, cmd: Command) -> CommandResult:
        # 模拟延迟
        lat_s = self.config["latency_ms"] / 1000.0
        time.sleep(lat_s + random.uniform(0, lat_s * 0.2))

        # 模拟随机失败
        if random.random() < self.config["fail_rate"]:
            return CommandResult.fail(cmd, "[仿真] 随机故障注入")

        # 更新内部状态
        ct = cmd.command_type
        p  = cmd.params

        if ct == CommandType.ARM:
            self._state["armed"] = True
        elif ct == CommandType.DISARM:
            self._state["armed"] = False
        elif ct == CommandType.EMERGENCY_STOP:
            self._state["armed"] = False
            self._state["speed"] = 0.0
        elif ct == CommandType.TAKEOFF:
            self._state["alt"]   = p.get("altitude", 10.0)
            self._state["speed"] = p.get("speed", 3.0)
            self._state["mode"]  = "GUIDED"
        elif ct == CommandType.LAND:
            self._state["alt"]   = 0.0
            self._state["speed"] = 0.0
            self._state["mode"]  = "LAND"
        elif ct == CommandType.RTL:
            self._state["mode"]  = "RTL"
        elif ct == CommandType.GOTO:
            self._state["lat"]   = p.get("lat", self._state["lat"])
            self._state["lon"]   = p.get("lon", self._state["lon"])
            self._state["alt"]   = p.get("alt", self._state["alt"])
            self._state["speed"] = p.get("speed", 5.0)
        elif ct == CommandType.SET_VELOCITY:
            vx = p.get("vx", 0); vy = p.get("vy", 0)
            self._state["speed"]   = math.hypot(vx, vy)
            self._state["heading"] = (
                math.degrees(math.atan2(vy, vx)) + 360
            ) % 360
        elif ct == CommandType.SET_MODE:
            self._state["mode"] = p.get("mode", "STABILIZE")
        elif ct in (CommandType.SIT, CommandType.STAND,
                    CommandType.WALK, CommandType.TROT):
            self._state["gait"] = ct.value.upper()
            self._state["speed"] = (
                p.get("speed", 1.0)
                if ct in (CommandType.WALK, CommandType.TROT) else 0.0
            )

        return CommandResult.ok(
            cmd, f"[仿真] {ct.value} 执行成功",
            data=dict(self._state)
        )

    def get_telemetry(self) -> BaseTelemetry:
        t  = time.time()
        s  = self._state
        dt = self.config["device_type"]

        # 模拟位置漂移
        s["lat"] += math.sin(t * 0.01) * 0.00001
        s["lon"] += math.cos(t * 0.01) * 0.00001
        s["battery"] = max(0, int(100 - self.uptime_s * 0.05))

        base = dict(
            device_id        = self.device_id,
            timestamp        = t,
            connection_state = ConnectionState.CONNECTED.value,
            latitude         = s["lat"],
            longitude        = s["lon"],
            altitude         = s["alt"],
            heading          = s["heading"],
            ground_speed     = s["speed"],
            battery_percent  = s["battery"],
            armed            = s["armed"],
            mode             = s["mode"],
            gps_fix_type     = 3,
            satellites_visible = 14,
            signal_strength  = 90,
        )

        if dt == "controller":
            return UAVTelemetry(**{
                k: v for k, v in base.items()
                if k in UAVTelemetry.__dataclass_fields__
            }, flight_mode=s["mode"])
        else:
            return RobotDogTelemetry(**{
                k: v for k, v in base.items()
                if k in RobotDogTelemetry.__dataclass_fields__
            }, gait_mode=s["gait"])

    def supported_commands(self) -> List[CommandType]:
        return self._ALL_COMMANDS

    def inject_fault(self, fail_rate: float) -> None:
        """动态注入故障率（测试用）"""
        self.config["fail_rate"] = max(0.0, min(1.0, fail_rate))
        self.logger.warning(f"故障注入: fail_rate={fail_rate:.0%}")