339 lines
9.3 KiB
Python
339 lines
9.3 KiB
Python
"""
|
||
flight_intent v1 校验与辅助（对齐 docs/FLIGHT_INTENT_SCHEMA_v1.md）。
|
||
|
||
兼容 Pydantic v2（field_validator / ConfigDict）。
|
||
|
||
互操作：部分云端会把「悬停 N 秒」写成 hover.args.duration；规范建议用 hover + wait.seconds。
|
||
解析时会折叠为 hover（无 duration）+ wait(seconds)，与机端执行一致。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
from dataclasses import dataclass
|
||
from typing import Any, List, Literal, Optional, Tuple, Union
|
||
|
||
from pydantic import BaseModel, ConfigDict, ValidationInfo, field_validator
|
||
|
||
_COORD_CAP = 10_000.0
|
||
|
||
|
||
def _check_coord(name: str, v: Optional[float]) -> Optional[float]:
|
||
if v is None:
|
||
return None
|
||
if not isinstance(v, (int, float)) or not math.isfinite(float(v)):
|
||
raise ValueError(f"{name} must be a finite number")
|
||
fv = float(v)
|
||
if abs(fv) > _COORD_CAP:
|
||
raise ValueError(f"{name} out of range (|.| <= {_COORD_CAP})")
|
||
return fv
|
||
|
||
|
||
# --- args -----------------------------------------------------------------
|
||
|
||
|
||
class TakeoffArgs(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
relative_altitude_m: Optional[float] = None
|
||
|
||
@field_validator("relative_altitude_m")
|
||
@classmethod
|
||
def _alt(cls, v: Optional[float]) -> Optional[float]:
|
||
if v is not None and v <= 0:
|
||
raise ValueError("relative_altitude_m must be > 0 when set")
|
||
return v
|
||
|
||
|
||
class EmptyArgs(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
|
||
class HoverHoldArgs(BaseModel):
|
||
"""hover / hold：规范仅 {}; 为兼容云端可带 duration（秒），解析后展开为 wait。"""
|
||
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
duration: Optional[float] = None
|
||
|
||
@field_validator("duration")
|
||
@classmethod
|
||
def _dur(cls, v: Optional[float]) -> Optional[float]:
|
||
if v is None:
|
||
return None
|
||
fv = float(v)
|
||
if not (0 < fv <= 3600):
|
||
raise ValueError("duration must satisfy 0 < duration <= 3600 when set")
|
||
return fv
|
||
|
||
|
||
class WaitArgs(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
seconds: float
|
||
|
||
@field_validator("seconds")
|
||
@classmethod
|
||
def _rng(cls, v: float) -> float:
|
||
if not (0 < v <= 3600):
|
||
raise ValueError("seconds must satisfy 0 < seconds <= 3600")
|
||
return v
|
||
|
||
|
||
class GotoArgs(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
frame: str
|
||
x: Optional[float] = None
|
||
y: Optional[float] = None
|
||
z: Optional[float] = None
|
||
|
||
@field_validator("frame")
|
||
@classmethod
|
||
def _frame(cls, v: str) -> str:
|
||
if v not in ("local_ned", "body_ned"):
|
||
raise ValueError('frame must be "local_ned" or "body_ned"')
|
||
return v
|
||
|
||
@field_validator("x", "y", "z")
|
||
@classmethod
|
||
def _coord(cls, v: Optional[float], info: ValidationInfo) -> Optional[float]:
|
||
return _check_coord(str(info.field_name), v)
|
||
|
||
|
||
# --- actions --------------------------------------------------------------
|
||
|
||
|
||
class ActionTakeoff(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
type: Literal["takeoff"] = "takeoff"
|
||
args: TakeoffArgs
|
||
|
||
|
||
class ActionLand(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
type: Literal["land"] = "land"
|
||
args: EmptyArgs
|
||
|
||
|
||
class ActionReturnHome(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
type: Literal["return_home"] = "return_home"
|
||
args: EmptyArgs
|
||
|
||
|
||
class ActionHover(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
type: Literal["hover"] = "hover"
|
||
args: HoverHoldArgs
|
||
|
||
|
||
class ActionHold(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
type: Literal["hold"] = "hold"
|
||
args: HoverHoldArgs
|
||
|
||
|
||
class ActionGoto(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
type: Literal["goto"] = "goto"
|
||
args: GotoArgs
|
||
|
||
|
||
class ActionWait(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
type: Literal["wait"] = "wait"
|
||
args: WaitArgs
|
||
|
||
|
||
FlightAction = Union[
|
||
ActionTakeoff,
|
||
ActionLand,
|
||
ActionReturnHome,
|
||
ActionHover,
|
||
ActionHold,
|
||
ActionGoto,
|
||
ActionWait,
|
||
]
|
||
|
||
|
||
class FlightIntentPayload(BaseModel):
|
||
model_config = ConfigDict(extra="forbid")
|
||
|
||
is_flight_intent: bool
|
||
version: int
|
||
actions: List[Any]
|
||
summary: str
|
||
trace_id: Optional[str] = None
|
||
|
||
@field_validator("is_flight_intent")
|
||
@classmethod
|
||
def _flag(cls, v: bool) -> bool:
|
||
if v is not True:
|
||
raise ValueError("is_flight_intent must be true")
|
||
return v
|
||
|
||
@field_validator("version")
|
||
@classmethod
|
||
def _ver(cls, v: int) -> int:
|
||
if v != 1:
|
||
raise ValueError("version must be 1")
|
||
return v
|
||
|
||
@field_validator("summary")
|
||
@classmethod
|
||
def _sum(cls, v: str) -> str:
|
||
if not (isinstance(v, str) and v.strip()):
|
||
raise ValueError("summary must be non-empty")
|
||
return v
|
||
|
||
@field_validator("trace_id")
|
||
@classmethod
|
||
def _tid(cls, v: Optional[str]) -> Optional[str]:
|
||
if v is not None and len(v) > 128:
|
||
raise ValueError("trace_id length must be <= 128")
|
||
return v
|
||
|
||
@field_validator("actions")
|
||
@classmethod
|
||
def _actions_nonempty(cls, v: List[Any]) -> List[Any]:
|
||
if not isinstance(v, list) or len(v) == 0:
|
||
raise ValueError("actions must be a non-empty array")
|
||
return v
|
||
|
||
|
||
@dataclass
|
||
class ValidatedFlightIntent:
|
||
summary: str
|
||
trace_id: Optional[str]
|
||
actions: List[FlightAction]
|
||
|
||
|
||
def _parse_one_action(raw: dict) -> FlightAction:
|
||
t = raw.get("type")
|
||
if not isinstance(t, str):
|
||
raise ValueError("action.type must be a string")
|
||
args = raw.get("args")
|
||
if not isinstance(args, dict):
|
||
raise ValueError("action.args must be an object")
|
||
if t == "takeoff":
|
||
return ActionTakeoff(args=TakeoffArgs.model_validate(args))
|
||
if t == "land":
|
||
return ActionLand(args=EmptyArgs.model_validate(args))
|
||
if t == "return_home":
|
||
return ActionReturnHome(args=EmptyArgs.model_validate(args))
|
||
if t == "hover":
|
||
return ActionHover(args=HoverHoldArgs.model_validate(args))
|
||
if t == "hold":
|
||
return ActionHold(args=HoverHoldArgs.model_validate(args))
|
||
if t == "goto":
|
||
return ActionGoto(args=GotoArgs.model_validate(args))
|
||
if t == "wait":
|
||
return ActionWait(args=WaitArgs.model_validate(args))
|
||
raise ValueError(f"unknown action.type: {t!r}")
|
||
|
||
|
||
def _expand_hover_duration(actions: List[FlightAction]) -> List[FlightAction]:
|
||
"""将 hover/hold 上附带的 duration 转为标准 wait(seconds)。"""
|
||
out: List[FlightAction] = []
|
||
for a in actions:
|
||
if isinstance(a, ActionHover):
|
||
d = a.args.duration
|
||
if d is not None:
|
||
out.append(ActionHover(args=HoverHoldArgs()))
|
||
out.append(ActionWait(args=WaitArgs(seconds=float(d))))
|
||
else:
|
||
out.append(a)
|
||
elif isinstance(a, ActionHold):
|
||
d = a.args.duration
|
||
if d is not None:
|
||
out.append(ActionHold(args=HoverHoldArgs()))
|
||
out.append(ActionWait(args=WaitArgs(seconds=float(d))))
|
||
else:
|
||
out.append(a)
|
||
else:
|
||
out.append(a)
|
||
return out
|
||
|
||
|
||
def parse_flight_intent_dict(data: dict) -> Tuple[Optional[ValidatedFlightIntent], List[str]]:
|
||
"""
|
||
L1–L3 校验。成功返回 (ValidatedFlightIntent, [])；失败返回 (None, [错误信息, ...])。
|
||
"""
|
||
errors: List[str] = []
|
||
try:
|
||
top = FlightIntentPayload.model_validate(data)
|
||
except Exception as e: # noqa: BLE001
|
||
return None, [str(e)]
|
||
|
||
parsed_actions: List[FlightAction] = []
|
||
for i, item in enumerate(top.actions):
|
||
if not isinstance(item, dict):
|
||
errors.append(f"actions[{i}] must be an object")
|
||
continue
|
||
try:
|
||
parsed_actions.append(_parse_one_action(item))
|
||
except Exception as e: # noqa: BLE001
|
||
errors.append(f"actions[{i}]: {e}")
|
||
|
||
if errors:
|
||
return None, errors
|
||
|
||
parsed_actions = _expand_hover_duration(parsed_actions)
|
||
|
||
if isinstance(parsed_actions[0], ActionWait):
|
||
return None, ["first action must not be wait (nothing to control yet)"]
|
||
|
||
return (
|
||
ValidatedFlightIntent(
|
||
summary=top.summary.strip(),
|
||
trace_id=top.trace_id,
|
||
actions=parsed_actions,
|
||
),
|
||
[],
|
||
)
|
||
|
||
|
||
def goto_action_to_command(action: ActionGoto, sequence_id: int) -> Tuple[Optional[Any], Optional[str]]:
|
||
"""
|
||
将单轴 goto 映射为现有 Socket Command。
|
||
返回 (Command | None, error_reason | None)。
|
||
"""
|
||
from voice_drone.core.command import Command
|
||
|
||
a = action.args
|
||
coords = [
|
||
("x", a.x),
|
||
("y", a.y),
|
||
("z", a.z),
|
||
]
|
||
active = [(name, val) for name, val in coords if val is not None and val != 0]
|
||
if len(active) == 0:
|
||
return None, "goto: all axes omit or zero (no-op)"
|
||
if len(active) > 1:
|
||
return (
|
||
None,
|
||
f"goto: multi-axis ({', '.join(n for n, _ in active)}) not sent via Socket "
|
||
"(use bridge or decompose)",
|
||
)
|
||
|
||
name, val = active[0]
|
||
dist = abs(float(val))
|
||
body_map = {
|
||
"x": ("forward", "backward"),
|
||
"y": ("right", "left"),
|
||
"z": ("down", "up"),
|
||
}
|
||
pos, neg = body_map[name]
|
||
cmd_name = pos if val > 0 else neg
|
||
cmd = Command.create(cmd_name, sequence_id, distance=dist)
|
||
cmd.fill_defaults()
|
||
return cmd, None
|