281 lines
13 KiB
Python
281 lines
13 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
无人系统控制指令模型
|
||
─────────────────────────────────────────────────────────────────
|
||
Command 指令基类（dataclass）
|
||
CommandResult 指令执行结果
|
||
CommandType 指令类型枚举（覆盖无人机/机器狗/通用）
|
||
CommandStatus 指令状态枚举
|
||
─────────────────────────────────────────────────────────────────
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import dataclasses
|
||
import time
|
||
import uuid
|
||
from dataclasses import dataclass, field
|
||
from enum import Enum
|
||
from typing import Any, Dict
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 指令类型枚举
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class CommandType(Enum):
|
||
# ── 通用指令 ──────────────────────────────────────────────────
|
||
ARM = "arm" # 解锁/上电
|
||
DISARM = "disarm" # 加锁/断电
|
||
START = "start" # 启动
|
||
STOP = "stop" # 停止
|
||
PAUSE = "pause" # 暂停
|
||
RESUME = "resume" # 恢复
|
||
RESET = "reset" # 重置
|
||
EMERGENCY_STOP = "emergency_stop" # 紧急停止
|
||
SET_MODE = "set_mode" # 设置飞行/运动模式
|
||
HEARTBEAT = "heartbeat" # 心跳保活
|
||
|
||
# ── 运动指令 ──────────────────────────────────────────────────
|
||
TAKEOFF = "takeoff" # 起飞（无人机）
|
||
LAND = "land" # 降落（无人机）
|
||
RTL = "rtl" # 返回起飞点
|
||
GOTO = "goto" # 飞往/走往目标点
|
||
SET_VELOCITY = "set_velocity" # 设置速度向量
|
||
SET_HEADING = "set_heading" # 设置航向
|
||
SET_ALTITUDE = "set_altitude" # 设置目标高度
|
||
FOLLOW_ROUTE = "follow_route" # 执行预设航线
|
||
LOITER = "loiter" # 盘旋/原地待命
|
||
|
||
# ── 载荷指令 ──────────────────────────────────────────────────
|
||
PAYLOAD_ON = "payload_on" # 载荷上电
|
||
PAYLOAD_OFF = "payload_off" # 载荷断电
|
||
CAMERA_SHOOT = "camera_shoot" # 拍照
|
||
CAMERA_RECORD = "camera_record" # 录像开关
|
||
GIMBAL_CONTROL = "gimbal_control" # 云台控制
|
||
|
||
# ── 机器狗专有 ────────────────────────────────────────────────
|
||
SIT = "sit" # 坐下
|
||
STAND = "stand" # 站立
|
||
WALK = "walk" # 行走
|
||
TROT = "trot" # 小跑
|
||
CLIMB_STAIRS = "climb_stairs" # 爬楼梯
|
||
SET_GAIT = "set_gait" # 设置步态
|
||
|
||
# ── 自定义扩展 ────────────────────────────────────────────────
|
||
CUSTOM = "custom" # 自定义指令
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 指令状态枚举
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class CommandStatus(Enum):
|
||
PENDING = "pending" # 待发送
|
||
SENT = "sent" # 已发送，等待确认
|
||
ACCEPTED = "accepted" # 设备已接受
|
||
EXECUTING = "executing" # 执行中
|
||
COMPLETED = "completed" # 执行完成
|
||
FAILED = "failed" # 执行失败
|
||
TIMEOUT = "timeout" # 超时未响应
|
||
REJECTED = "rejected" # 设备拒绝
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 指令基类
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class Command:
|
||
"""
|
||
无人系统控制指令
|
||
|
||
所有指令均继承此类，通过 params 字典传递指令参数。
|
||
子类可新增强类型字段以提升可读性。
|
||
"""
|
||
|
||
# ── 标识 ──────────────────────────────────────────────────────
|
||
command_id: str = field(
|
||
default_factory=lambda: str(uuid.uuid4())[:8]
|
||
)
|
||
command_type: CommandType = CommandType.CUSTOM
|
||
target_id: str = "" # 目标设备 ID
|
||
|
||
# ── 参数 ──────────────────────────────────────────────────────
|
||
params: Dict[str, Any] = field(default_factory=dict)
|
||
|
||
# ── 时序 ──────────────────────────────────────────────────────
|
||
created_at: float = field(default_factory=time.time)
|
||
timeout_s: float = 10.0 # 超时时间（秒）
|
||
priority: int = 0 # 优先级（越大越高）
|
||
|
||
# ── 状态 ──────────────────────────────────────────────────────
|
||
status: CommandStatus = CommandStatus.PENDING
|
||
|
||
# ── 序列化 ────────────────────────────────────────────────────
|
||
|
||
def to_dict(self) -> Dict[str, Any]:
|
||
d = dataclasses.asdict(self)
|
||
d["command_type"] = self.command_type.value
|
||
d["status"] = self.status.value
|
||
return d
|
||
|
||
def __repr__(self) -> str:
|
||
return (f"Command({self.command_type.value}"
|
||
f"[{self.command_id}]"
|
||
f"→{self.target_id}"
|
||
f" {self.status.value})")
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 常用指令工厂方法
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class Commands:
|
||
"""常用指令快速构造工厂"""
|
||
|
||
@staticmethod
|
||
def guide(target_id: str):
|
||
return Command(command_type=CommandType.CUSTOM,
|
||
target_id=target_id)
|
||
|
||
@staticmethod
|
||
def arm(target_id: str, **kw) -> Command:
|
||
return Command(command_type=CommandType.ARM,
|
||
target_id=target_id, **kw)
|
||
|
||
@staticmethod
|
||
def disarm(target_id: str, **kw) -> Command:
|
||
return Command(command_type=CommandType.DISARM,
|
||
target_id=target_id, **kw)
|
||
|
||
@staticmethod
|
||
def takeoff(target_id: str, altitude: float,
|
||
speed: float = 3.0, **kw) -> Command:
|
||
return Command(command_type=CommandType.TAKEOFF,
|
||
target_id=target_id,
|
||
params={"altitude": altitude, "speed": speed},
|
||
**kw)
|
||
|
||
@staticmethod
|
||
def land(target_id: str, **kw) -> Command:
|
||
return Command(command_type=CommandType.LAND,
|
||
target_id=target_id, **kw)
|
||
|
||
@staticmethod
|
||
def rtl(target_id: str, **kw) -> Command:
|
||
return Command(command_type=CommandType.RTL,
|
||
target_id=target_id, **kw)
|
||
|
||
@staticmethod
|
||
def goto(target_id: str, lat: float, lon: float,
|
||
alt: float, speed: float = 5.0, **kw) -> Command:
|
||
return Command(command_type=CommandType.GOTO,
|
||
target_id=target_id,
|
||
params={"lat": lat, "lon": lon,
|
||
"alt": alt, "speed": speed},
|
||
**kw)
|
||
|
||
@staticmethod
|
||
def set_velocity(target_id: str,
|
||
vx: float, vy: float, vz: float = 0.0,
|
||
**kw) -> Command:
|
||
return Command(command_type=CommandType.SET_VELOCITY,
|
||
target_id=target_id,
|
||
params={"vx": vx, "vy": vy, "vz": vz},
|
||
**kw)
|
||
|
||
@staticmethod
|
||
def emergency_stop(target_id: str, **kw) -> Command:
|
||
return Command(command_type=CommandType.EMERGENCY_STOP,
|
||
target_id=target_id,
|
||
priority=999, **kw)
|
||
|
||
@staticmethod
|
||
def set_mode(target_id: str, mode: str, **kw) -> Command:
|
||
return Command(command_type=CommandType.SET_MODE,
|
||
target_id=target_id,
|
||
params={"mode": mode}, **kw)
|
||
|
||
@staticmethod
|
||
def sit(target_id: str, **kw) -> Command:
|
||
return Command(command_type=CommandType.SIT,
|
||
target_id=target_id, **kw)
|
||
|
||
@staticmethod
|
||
def stand(target_id: str, **kw) -> Command:
|
||
return Command(command_type=CommandType.STAND,
|
||
target_id=target_id, **kw)
|
||
|
||
@staticmethod
|
||
def walk(target_id: str, speed: float = 1.0,
|
||
heading: float = 0.0, **kw) -> Command:
|
||
return Command(command_type=CommandType.WALK,
|
||
target_id=target_id,
|
||
params={"speed": speed, "heading": heading},
|
||
**kw)
|
||
|
||
@staticmethod
|
||
def custom(target_id: str, model: int,
|
||
**params) -> Command:
|
||
return Command(command_type=CommandType.CUSTOM,
|
||
target_id=target_id,
|
||
params={"model": model, **params})
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 指令执行结果
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class CommandResult:
|
||
"""指令执行结果"""
|
||
command_id: str = ""
|
||
command_type: str = ""
|
||
status: CommandStatus = CommandStatus.PENDING
|
||
success: bool = False
|
||
message: str = ""
|
||
data: Dict[str, Any] = field(default_factory=dict)
|
||
elapsed_ms: float = 0.0
|
||
timestamp: float = field(default_factory=time.time)
|
||
|
||
@classmethod
|
||
def ok(cls, cmd: Command, message: str = "成功",
|
||
data: dict = None, elapsed_ms: float = 0.0) -> "CommandResult":
|
||
return cls(
|
||
command_id = cmd.command_id,
|
||
command_type = cmd.command_type.value,
|
||
status = CommandStatus.COMPLETED,
|
||
success = True,
|
||
message = message,
|
||
data = data or {},
|
||
elapsed_ms = elapsed_ms,
|
||
)
|
||
|
||
@classmethod
|
||
def fail(cls, cmd: Command, message: str = "失败",
|
||
elapsed_ms: float = 0.0) -> "CommandResult":
|
||
return cls(
|
||
command_id = cmd.command_id,
|
||
command_type = cmd.command_type.value,
|
||
status = CommandStatus.FAILED,
|
||
success = False,
|
||
message = message,
|
||
elapsed_ms = elapsed_ms,
|
||
)
|
||
|
||
@classmethod
|
||
def timeout(cls, cmd: Command) -> "CommandResult":
|
||
return cls(
|
||
command_id = cmd.command_id,
|
||
command_type = cmd.command_type.value,
|
||
status = CommandStatus.TIMEOUT,
|
||
success = False,
|
||
message = f"指令超时（>{cmd.timeout_s}s）",
|
||
)
|
||
|
||
def __repr__(self) -> str:
|
||
mark = "✅" if self.success else "❌"
|
||
return (f"{mark} CommandResult("
|
||
f"{self.command_type}[{self.command_id}] "
|
||
f"{self.status.value}: {self.message})") |