344 lines
12 KiB
Python
344 lines
12 KiB
Python
|
from __future__ import annotations
|
|||
|
|
|
|||
|
|
import sys
|
|||
|
|
|
|||
|
|
from PyQt5.QtWidgets import QApplication
|
|||
|
|
|
|||
|
|
from uas.uas_control.base_controller import BaseUASController
|
|||
|
|
from uas.uas_control.command import CommandType, Commands
|
|||
|
|
from uas.uas_control.controllers.robot_dog_controller import SEP, RobotDogController
|
|||
|
|
from uas.uas_control.controllers.uav_controller import UAVController
|
|||
|
|
from uas.uas_control.controllers.usv_controller import USVController
|
|||
|
|
from uas.uas_control.manager import UASManager
|
|||
|
|
from uas.uas_control.protocols.mavlink_adapter import MAVLinkAdapter
|
|||
|
|
from uas.uas_control.protocols.ros_adapter import ROSAdapter
|
|||
|
|
from uas.uas_control.protocols.simulation_adapter import SimulationAdapter
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ═════════════════════════════════════════════════════════════════
|
|||
|
|
# 演示函数
|
|||
|
|
# ═════════════════════════════════════════════════════════════════
|
|||
|
|
|
|||
|
|
def demo_uav_mavlink(app: QApplication):
|
|||
|
|
print(f"\n{SEP}\n 演示1：无人机 × MAVLink 适配器\n{SEP}")
|
|||
|
|
|
|||
|
|
adapter = MAVLinkAdapter(
|
|||
|
|
device_id = "UAV-001",
|
|||
|
|
connection_string = "udp:127.0.0.1:14550",
|
|||
|
|
)
|
|||
|
|
uav = UAVController("UAV-001", adapter)
|
|||
|
|
|
|||
|
|
results, errors = [], []
|
|||
|
|
uav.command_result.connect(lambda r: results.append(r))
|
|||
|
|
uav.error_occurred.connect(lambda m: errors.append(m))
|
|||
|
|
|
|||
|
|
assert uav.connect(), "连接应成功"
|
|||
|
|
print(f" 连接状态 : {uav.is_connected}")
|
|||
|
|
print(f" 适配器 : {adapter}")
|
|||
|
|
|
|||
|
|
# 标准起飞流程
|
|||
|
|
flow = [
|
|||
|
|
("解锁", uav.arm()),
|
|||
|
|
("起飞50m", uav.takeoff(altitude=50.0, speed=3.0)),
|
|||
|
|
("前往目标", uav.goto(39.95, 116.45, 100.0, speed=10.0)),
|
|||
|
|
("云台俯仰", uav.set_gimbal(pitch=-45.0)),
|
|||
|
|
("拍照", uav.camera_shoot()),
|
|||
|
|
("录像开始", uav.camera_record(start=True)),
|
|||
|
|
("盘旋", uav.loiter(radius=80.0)),
|
|||
|
|
("录像停止", uav.camera_record(start=False)),
|
|||
|
|
("返航", uav.rtl()),
|
|||
|
|
("降落", uav.land()),
|
|||
|
|
("加锁", uav.disarm()),
|
|||
|
|
]
|
|||
|
|
for name, result in flow:
|
|||
|
|
mark = "✅" if result.success else "❌"
|
|||
|
|
print(f" {mark} {name:8s}: {result.message[:45]}")
|
|||
|
|
|
|||
|
|
# 遥测
|
|||
|
|
app.processEvents()
|
|||
|
|
telem = uav.get_telemetry()
|
|||
|
|
if telem:
|
|||
|
|
print(f"\n 遥测快照 : {telem}")
|
|||
|
|
|
|||
|
|
print(f" 指令历史 : {len(uav.command_history)} 条")
|
|||
|
|
uav.disconnect()
|
|||
|
|
print(f" ✅ MAVLink 适配器演示通过\n")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def demo_robot_dog_ros(app: QApplication):
|
|||
|
|
print(f"\n{SEP}\n 演示2：机器狗 × ROS2 适配器\n{SEP}")
|
|||
|
|
|
|||
|
|
adapter = ROSAdapter(
|
|||
|
|
device_id = "DOG-001",
|
|||
|
|
ros_version = 2,
|
|||
|
|
namespace = "spot",
|
|||
|
|
device_type = "robot_dog",
|
|||
|
|
)
|
|||
|
|
dog = RobotDogController("DOG-001", adapter)
|
|||
|
|
|
|||
|
|
results = []
|
|||
|
|
dog.command_result.connect(lambda r: results.append(r))
|
|||
|
|
|
|||
|
|
dog.connect()
|
|||
|
|
print(f" 连接状态 : {dog.is_connected}")
|
|||
|
|
print(f" 适配器 : {adapter}")
|
|||
|
|
|
|||
|
|
# 机器狗动作序列
|
|||
|
|
actions = [
|
|||
|
|
("站立", dog.stand()),
|
|||
|
|
("设置高度", dog.set_body_height(0.6)),
|
|||
|
|
("行走", dog.walk(speed=1.5, heading=90.0)),
|
|||
|
|
("小跑", dog.trot(speed=2.5)),
|
|||
|
|
("设置步态", dog.set_gait("TROT")),
|
|||
|
|
("爬楼梯", dog.climb_stairs()),
|
|||
|
|
("自定义", dog.custom_command("led_blink", color="red", hz=2)),
|
|||
|
|
("坐下", dog.sit()),
|
|||
|
|
]
|
|||
|
|
for name, result in actions:
|
|||
|
|
mark = "✅" if result.success else "❌"
|
|||
|
|
print(f" {mark} {name:8s}: {result.message[:45]}")
|
|||
|
|
|
|||
|
|
app.processEvents()
|
|||
|
|
telem = dog.get_telemetry()
|
|||
|
|
if telem:
|
|||
|
|
print(f"\n 遥测快照 : {telem}")
|
|||
|
|
|
|||
|
|
dog.disconnect()
|
|||
|
|
print(f" ✅ ROS2 适配器演示通过\n")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def demo_protocol_hotswap(app: QApplication):
|
|||
|
|
print(f"\n{SEP}\n 演示3：协议热切换（MAVLink → ROS2）\n{SEP}")
|
|||
|
|
|
|||
|
|
# 初始使用 MAVLink
|
|||
|
|
mav_adapter = MAVLinkAdapter("UAV-002", "udp:127.0.0.1:14551")
|
|||
|
|
uav = UAVController("UAV-002", mav_adapter)
|
|||
|
|
uav.connect()
|
|||
|
|
r1 = uav.arm()
|
|||
|
|
print(f" MAVLink 解锁: {'✅' if r1.success else '❌'} {r1.message[:40]}")
|
|||
|
|
print(f" 当前适配器 : {uav._adapter.__class__.__name__}")
|
|||
|
|
|
|||
|
|
# 热切换到 ROS2
|
|||
|
|
ros_adapter = ROSAdapter(
|
|||
|
|
"UAV-002", ros_version=2,
|
|||
|
|
namespace="uav2", device_type="controller"
|
|||
|
|
)
|
|||
|
|
uav.switch_adapter(ros_adapter)
|
|||
|
|
uav.connect()
|
|||
|
|
r2 = uav.goto(39.9, 116.4, 80.0)
|
|||
|
|
print(f" ROS2 GOTO : {'✅' if r2.success else '❌'} {r2.message[:40]}")
|
|||
|
|
print(f" 切换后适配器: {uav._adapter.__class__.__name__}")
|
|||
|
|
|
|||
|
|
# 再切换回仿真适配器
|
|||
|
|
sim_adapter = SimulationAdapter("UAV-002", device_type="controller")
|
|||
|
|
uav.switch_adapter(sim_adapter)
|
|||
|
|
uav.connect()
|
|||
|
|
r3 = uav.takeoff(100.0)
|
|||
|
|
print(f" 仿真 起飞 : {'✅' if r3.success else '❌'} {r3.message[:40]}")
|
|||
|
|
print(f" 最终适配器 : {uav._adapter.__class__.__name__}")
|
|||
|
|
|
|||
|
|
uav.disconnect()
|
|||
|
|
print(f" ✅ 协议热切换演示通过\n")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def demo_multi_device_manager(app: QApplication):
|
|||
|
|
print(f"\n{SEP}\n 演示4：多设备统一管理（UASManager）\n{SEP}")
|
|||
|
|
|
|||
|
|
manager = UASManager()
|
|||
|
|
alerts = []
|
|||
|
|
manager.alert.connect(lambda did, msg: alerts.append(f"[{did}] {msg}"))
|
|||
|
|
|
|||
|
|
# 注册 3 架无人机 + 2 只机器狗 + 1 艘无人船
|
|||
|
|
devices = [
|
|||
|
|
UAVController("UAV-A",
|
|||
|
|
SimulationAdapter("UAV-A", device_type="controller")),
|
|||
|
|
UAVController("UAV-B",
|
|||
|
|
SimulationAdapter("UAV-B", device_type="controller")),
|
|||
|
|
UAVController("UAV-C",
|
|||
|
|
MAVLinkAdapter("UAV-C", "udp:127.0.0.1:14552")),
|
|||
|
|
RobotDogController("DOG-A",
|
|||
|
|
SimulationAdapter("DOG-A", device_type="robot_dog")),
|
|||
|
|
RobotDogController("DOG-B",
|
|||
|
|
ROSAdapter("DOG-B", ros_version=2,
|
|||
|
|
namespace="dog_b", device_type="robot_dog")),
|
|||
|
|
USVController("USV-A",
|
|||
|
|
SimulationAdapter("USV-A", device_type="controller")),
|
|||
|
|
]
|
|||
|
|
for d in devices:
|
|||
|
|
manager.register(d)
|
|||
|
|
|
|||
|
|
print(f" 已注册设备: {manager.device_count} 台")
|
|||
|
|
|
|||
|
|
# 批量连接
|
|||
|
|
conn_results = manager.connect_all()
|
|||
|
|
ok_count = sum(1 for v in conn_results.values() if v)
|
|||
|
|
print(f" 批量连接 : {ok_count}/{len(conn_results)} 成功")
|
|||
|
|
|
|||
|
|
# 广播：所有无人机解锁
|
|||
|
|
print(f"\n 广播：所有无人机解锁")
|
|||
|
|
arm_results = manager.broadcast(
|
|||
|
|
lambda did: Commands.arm(did), device_type="controller"
|
|||
|
|
)
|
|||
|
|
for did, r in arm_results.items():
|
|||
|
|
print(f" {'✅' if r.success else '❌'} {did}: {r.message[:35]}")
|
|||
|
|
|
|||
|
|
# 广播：所有机器狗站立
|
|||
|
|
print(f"\n 广播：所有机器狗站立")
|
|||
|
|
stand_results = manager.broadcast(
|
|||
|
|
lambda did: Commands.stand(did), device_type="robot_dog"
|
|||
|
|
)
|
|||
|
|
for did, r in stand_results.items():
|
|||
|
|
print(f" {'✅' if r.success else '❌'} {did}: {r.message[:35]}")
|
|||
|
|
|
|||
|
|
# 单独控制
|
|||
|
|
print(f"\n 单独控制 UAV-A 起飞")
|
|||
|
|
uav_a: UAVController = manager.get("UAV-A")
|
|||
|
|
r = uav_a.takeoff(altitude=80.0)
|
|||
|
|
print(f" {'✅' if r.success else '❌'} UAV-A 起飞: {r.message[:40]}")
|
|||
|
|
|
|||
|
|
print(f"\n 单独控制 DOG-A 行走")
|
|||
|
|
dog_a: RobotDogController = manager.get("DOG-A")
|
|||
|
|
r = dog_a.walk(speed=2.0, heading=45.0)
|
|||
|
|
print(f" {'✅' if r.success else '❌'} DOG-A 行走: {r.message[:40]}")
|
|||
|
|
|
|||
|
|
# 遥测快照
|
|||
|
|
app.processEvents()
|
|||
|
|
print(f"\n 遥测快照（所有设备）:")
|
|||
|
|
for did, telem in manager.get_all_telemetry().items():
|
|||
|
|
if telem:
|
|||
|
|
print(f" {did:8s} bat={telem.battery_percent:3d}% "
|
|||
|
|
f"spd={telem.ground_speed:.1f}m/s "
|
|||
|
|
f"pos=({telem.latitude:.4f},{telem.longitude:.4f})")
|
|||
|
|
|
|||
|
|
# 协议热切换（通过 Manager）
|
|||
|
|
print(f"\n UAV-A 协议热切换: Simulation → MAVLink")
|
|||
|
|
new_adapter = MAVLinkAdapter("UAV-A", "udp:127.0.0.1:14553")
|
|||
|
|
manager.switch_protocol("UAV-A", new_adapter)
|
|||
|
|
r = uav_a.goto(39.95, 116.45, 100.0)
|
|||
|
|
print(f" {'✅' if r.success else '❌'} UAV-A GOTO (MAVLink): "
|
|||
|
|
f"{r.message[:35]}")
|
|||
|
|
|
|||
|
|
# 紧急停止所有
|
|||
|
|
print(f"\n 触发：紧急停止所有设备")
|
|||
|
|
stop_results = manager.emergency_stop_all()
|
|||
|
|
ok = sum(1 for r in stop_results.values() if r.success)
|
|||
|
|
print(f" 紧急停止: {ok}/{len(stop_results)} 成功")
|
|||
|
|
|
|||
|
|
# 状态报告
|
|||
|
|
print(f"\n{manager.status_report()}")
|
|||
|
|
|
|||
|
|
manager.disconnect_all()
|
|||
|
|
print(f" ✅ 多设备管理演示通过\n")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def demo_fault_injection(app: QApplication):
|
|||
|
|
print(f"\n{SEP}\n 演示5：故障注入与容错\n{SEP}")
|
|||
|
|
|
|||
|
|
adapter = SimulationAdapter(
|
|||
|
|
"UAV-FAULT", device_type="controller",
|
|||
|
|
fail_rate=0.0, latency_ms=10.0
|
|||
|
|
)
|
|||
|
|
uav = UAVController("UAV-FAULT", adapter)
|
|||
|
|
uav.connect()
|
|||
|
|
|
|||
|
|
errors = []
|
|||
|
|
uav.error_occurred.connect(lambda m: errors.append(m))
|
|||
|
|
|
|||
|
|
# 正常发送
|
|||
|
|
r = uav.arm()
|
|||
|
|
print(f" 正常模式 ARM: {'✅' if r.success else '❌'} {r.message[:40]}")
|
|||
|
|
|
|||
|
|
# 注入 80% 故障率
|
|||
|
|
adapter.inject_fault(0.8)
|
|||
|
|
print(f"\n 注入故障率 80%，连续发送 10 条指令:")
|
|||
|
|
success_cnt = 0
|
|||
|
|
for i in range(10):
|
|||
|
|
r = uav.goto(39.9 + i*0.001, 116.4, 50.0)
|
|||
|
|
if r.success:
|
|||
|
|
success_cnt += 1
|
|||
|
|
print(f" [{i+1:02d}] {'✅' if r.success else '❌'} "
|
|||
|
|
f"{r.message[:40]}")
|
|||
|
|
|
|||
|
|
print(f"\n 成功率: {success_cnt}/10 "
|
|||
|
|
f"（期望约 2/10，实际随机）")
|
|||
|
|
|
|||
|
|
# 恢复正常
|
|||
|
|
adapter.inject_fault(0.0)
|
|||
|
|
r = uav.emergency_stop()
|
|||
|
|
print(f"\n 恢复后 EMERGENCY_STOP: "
|
|||
|
|
f"{'✅' if r.success else '❌'} {r.message[:40]}")
|
|||
|
|
|
|||
|
|
# 回调注册
|
|||
|
|
cb_results = []
|
|||
|
|
uav.register_callback(
|
|||
|
|
CommandType.GOTO,
|
|||
|
|
lambda r: cb_results.append(r)
|
|||
|
|
)
|
|||
|
|
uav.goto(39.9, 116.4, 50.0)
|
|||
|
|
print(f"\n 指令回调触发: {len(cb_results)} 次")
|
|||
|
|
assert len(cb_results) == 1
|
|||
|
|
|
|||
|
|
uav.disconnect()
|
|||
|
|
print(f" ✅ 故障注入演示通过\n")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def demo_custom_extension(app: QApplication):
|
|||
|
|
print(f"\n{SEP}\n 演示6：自定义扩展（无人船 USV）\n{SEP}")
|
|||
|
|
|
|||
|
|
# USVController 继承 BaseUASController，
|
|||
|
|
# 复用 SimulationAdapter，无需修改任何底层代码
|
|||
|
|
adapter = SimulationAdapter("USV-001", device_type="controller")
|
|||
|
|
usv = USVController("USV-001", adapter)
|
|||
|
|
usv.connect()
|
|||
|
|
|
|||
|
|
actions = [
|
|||
|
|
("差速推进", usv.set_throttle(left=0.8, right=0.6)),
|
|||
|
|
("前往目标", usv.goto(22.5, 113.9, 0.0, speed=5.0)),
|
|||
|
|
("水面巡逻", usv.patrol([
|
|||
|
|
(22.50, 113.90, 0.0, 5.0),
|
|||
|
|
(22.52, 113.92, 0.0, 5.0),
|
|||
|
|
(22.54, 113.90, 0.0, 5.0),
|
|||
|
|
])),
|
|||
|
|
("抛锚", usv.anchor()),
|
|||
|
|
("紧急停止", usv.emergency_stop()),
|
|||
|
|
]
|
|||
|
|
for name, result in actions:
|
|||
|
|
mark = "✅" if result.success else "❌"
|
|||
|
|
print(f" {mark} {name:8s}: {result.message[:45]}")
|
|||
|
|
|
|||
|
|
print(f"\n USVController 继承关系验证:")
|
|||
|
|
print(f" isinstance(usv, BaseUASController) = "
|
|||
|
|
f"{isinstance(usv, BaseUASController)}")
|
|||
|
|
print(f" device_type = {usv.device_type}")
|
|||
|
|
print(f" adapter = {usv._adapter.__class__.__name__}")
|
|||
|
|
|
|||
|
|
usv.disconnect()
|
|||
|
|
print(f" ✅ 自定义扩展演示通过\n")
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ═════════════════════════════════════════════════════════════════
|
|||
|
|
# 主入口
|
|||
|
|
# ═════════════════════════════════════════════════════════════════
|
|||
|
|
|
|||
|
|
def main():
|
|||
|
|
app = QApplication(sys.argv)
|
|||
|
|
|
|||
|
|
demo_uav_mavlink(app)
|
|||
|
|
demo_robot_dog_ros(app)
|
|||
|
|
demo_protocol_hotswap(app)
|
|||
|
|
demo_multi_device_manager(app)
|
|||
|
|
demo_fault_injection(app)
|
|||
|
|
demo_custom_extension(app)
|
|||
|
|
|
|||
|
|
print(f"{SEP}")
|
|||
|
|
print(f" ✅ 所有演示通过！无人系统控制层模块运行正常")
|
|||
|
|
print(f" 模块统计:")
|
|||
|
|
print(f" 协议适配器 : MAVLinkAdapter / ROSAdapter / SimulationAdapter")
|
|||
|
|
print(f" 控制器 : UAVController / RobotDogController / USVController")
|
|||
|
|
print(f" 管理器 : UASManager（支持 {6} 台设备并发）")
|
|||
|
|
print(f" 支持指令数 : {len(CommandType)} 种")
|
|||
|
|
print(f"{SEP}\n")
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
main()
|