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