103 lines
3.5 KiB
Python
103 lines
3.5 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
无人系统控制器实现 + 多设备管理器 + 完整演示 — 完整版
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import logging
|
||
|
||
from uas.uas_control import command_registry
|
||
from uas.uas_control.base_controller import BaseUASController
|
||
from uas.uas_control.command import Command, CommandResult, CommandType, Commands
|
||
from uas.uas_control.telemetry import BaseTelemetry, RobotDogTelemetry
|
||
|
||
logging.basicConfig(
|
||
level = logging.INFO,
|
||
format = "%(asctime)s [%(levelname)-8s] %(name)s - %(message)s",
|
||
)
|
||
logger = logging.getLogger("main")
|
||
|
||
SEP = "═" * 65
|
||
SEP2 = "─" * 65
|
||
|
||
|
||
|
||
|
||
# ═════════════════════════════════════════════════════════════════
|
||
# 机器狗控制器
|
||
# ═════════════════════════════════════════════════════════════════
|
||
|
||
class RobotDogController(BaseUASController):
|
||
"""
|
||
机器狗控制器
|
||
|
||
扩展机器狗专有 API：
|
||
sit / stand / walk / trot / set_gait
|
||
set_body_height / climb_stairs
|
||
"""
|
||
|
||
@property
|
||
def device_type(self) -> str:
|
||
return "robot_dog"
|
||
|
||
# ── 机器狗专有 API ────────────────────────────────────────────
|
||
@command_registry.register
|
||
def sit(self) -> CommandResult:
|
||
return self.send(Commands.sit(self.device_id))
|
||
|
||
@command_registry.register
|
||
def stand(self) -> CommandResult:
|
||
return self.send(Commands.stand(self.device_id))
|
||
|
||
@command_registry.register
|
||
def walk(self, speed: float = 1.0,
|
||
heading: float = 0.0) -> CommandResult:
|
||
return self.send(Commands.walk(self.device_id, speed, heading))
|
||
|
||
@command_registry.register
|
||
def trot(self, speed: float = 2.0) -> CommandResult:
|
||
return self.send(Command(
|
||
command_type = CommandType.TROT,
|
||
target_id = self.device_id,
|
||
params = {"speed": speed},
|
||
))
|
||
|
||
@command_registry.register
|
||
def set_gait(self, gait: str) -> CommandResult:
|
||
"""设置步态: WALK / TROT / BOUND / CRAWL"""
|
||
return self.send(Command(
|
||
command_type = CommandType.SET_GAIT,
|
||
target_id = self.device_id,
|
||
params = {"gait": gait},
|
||
))
|
||
|
||
@command_registry.register
|
||
def set_body_height(self, height: float) -> CommandResult:
|
||
return self.send(Commands.custom(
|
||
self.device_id, "set_body_height", height=height
|
||
))
|
||
|
||
@command_registry.register
|
||
def climb_stairs(self) -> CommandResult:
|
||
return self.send(Command(
|
||
command_type = CommandType.CLIMB_STAIRS,
|
||
target_id = self.device_id,
|
||
))
|
||
|
||
# ── 遥测钩子 ──────────────────────────────────────────────────
|
||
|
||
def _on_telemetry(self, telem: BaseTelemetry) -> None:
|
||
if not isinstance(telem, RobotDogTelemetry):
|
||
return
|
||
if 0 < telem.battery_percent < 15:
|
||
self.error_occurred.emit(
|
||
f"{self.device_id}: 电量严重不足 {telem.battery_percent}%"
|
||
)
|
||
if telem.obstacle_detected:
|
||
self.error_occurred.emit(
|
||
f"{self.device_id}: 检测到障碍物，"
|
||
f"距离={telem.nearest_obstacle_m:.1f}m"
|
||
)
|
||
|