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