435 lines
17 KiB
Python
435 lines
17 KiB
Python
|
# -*- coding: utf-8 -*-
|
|||
|
|
from __future__ import annotations
|
|||
|
|
|
|||
|
|
import logging
|
|||
|
|
import queue
|
|||
|
|
import threading
|
|||
|
|
import traceback
|
|||
|
|
from enum import Enum
|
|||
|
|
from time import sleep
|
|||
|
|
from typing import Optional, Tuple
|
|||
|
|
|
|||
|
|
from PyQt5.QtCore import QObject, pyqtSlot, pyqtSignal, QThread
|
|||
|
|
|
|||
|
|
from uas.action.action_executor import action_executor
|
|||
|
|
from uas.event.event_bus import event_bus
|
|||
|
|
from uas.event.handlers.device_handlers import DeviceErrorHandler, DeviceTimeoutHandler, EquipmentFaultHandler
|
|||
|
|
from uas.task.task_executor import task_executor
|
|||
|
|
from uas.task.task_model import Task, TaskState
|
|||
|
|
from uas.uas_control.base_controller import BaseUASController
|
|||
|
|
|
|||
|
|
logger = logging.getLogger(__name__)
|
|||
|
|
|
|||
|
|
class AgentState(Enum):
|
|||
|
|
IDLE = "IDLE"
|
|||
|
|
READY = "READY"
|
|||
|
|
RUNNING = "RUNNING"
|
|||
|
|
PAUSED = "PAUSED"
|
|||
|
|
STOPPED = "STOPPED"
|
|||
|
|
SWITCHING = "SWITCHING"
|
|||
|
|
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
# 装备模型抽象基类
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
|
|||
|
|
class AgentTaskWorker(QObject):
|
|||
|
|
# 信号：用于向主线程发送结果
|
|||
|
|
finished = pyqtSignal()
|
|||
|
|
result = pyqtSignal(object) # 可传数据
|
|||
|
|
|
|||
|
|
def __init__(self, agent:"Agent", dt:int):
|
|||
|
|
super().__init__()
|
|||
|
|
self.agent = agent
|
|||
|
|
self.dt = dt
|
|||
|
|
|
|||
|
|
@pyqtSlot()
|
|||
|
|
def do_work(self):
|
|||
|
|
"""真正的耗时任务"""
|
|||
|
|
try:
|
|||
|
|
# 执行你的任务
|
|||
|
|
print("线程开始执行")
|
|||
|
|
res = self._start_task()
|
|||
|
|
# 发送结果
|
|||
|
|
self.result.emit(res)
|
|||
|
|
finally:
|
|||
|
|
self.finished.emit()
|
|||
|
|
|
|||
|
|
def _start_task(self):
|
|||
|
|
self.agent._model.start()
|
|||
|
|
self.agent.state = AgentState.RUNNING
|
|||
|
|
while self.agent.state != AgentState.STOPPED and not self.agent._task_queue.empty():
|
|||
|
|
self.agent.current_task = self.agent._task_queue.get()
|
|||
|
|
priority, task = self.agent.current_task
|
|||
|
|
assert isinstance(task, Task)
|
|||
|
|
self.agent._planning_task(priority, task)
|
|||
|
|
task_executor.submit(task)
|
|||
|
|
|
|||
|
|
while task.state in(TaskState.RUNNING, TaskState.PLANNED, TaskState.READY):
|
|||
|
|
# 如果需要切换任务
|
|||
|
|
if self.agent.from_to(AgentState.RUNNING, AgentState.SWITCHING):
|
|||
|
|
self.agent._save_task(priority, task)
|
|||
|
|
task_executor.cancel(task.task_id)
|
|||
|
|
break
|
|||
|
|
# 如果需要停止任务
|
|||
|
|
elif self.agent.from_to(AgentState.RUNNING, AgentState.STOPPED):
|
|||
|
|
task_executor.cancel(task.task_id)
|
|||
|
|
break
|
|||
|
|
elif self.agent.from_to(AgentState.RUNNING, AgentState.PAUSED):
|
|||
|
|
task_executor.pause(task.task_id)
|
|||
|
|
elif self.agent.from_to(AgentState.PAUSED, AgentState.RUNNING):
|
|||
|
|
task_executor.resume(task.task_id)
|
|||
|
|
try:
|
|||
|
|
if task.state == TaskState.RUNNING and self.agent.state == AgentState.RUNNING:
|
|||
|
|
result = self.agent._model.step(self.dt)
|
|||
|
|
# 控制仿真速率（实时：sleep = dt；加速：sleep < dt）
|
|||
|
|
sleep(max(.050, (self.dt/1000) * 0.200))
|
|||
|
|
except Exception as e:
|
|||
|
|
traceback.print_exc()
|
|||
|
|
print(f"[SimThread] 仿真异常: {e}")
|
|||
|
|
break
|
|||
|
|
self.agent.state = AgentState.IDLE
|
|||
|
|
self.agent.current_task = None
|
|||
|
|
self.agent._model.stop()
|
|||
|
|
|
|||
|
|
|
|||
|
|
def _start_task2(self):
|
|||
|
|
self.agent.state = AgentState.RUNNING
|
|||
|
|
while self.agent.state != AgentState.STOPPED and not self.agent._task_queue.empty():
|
|||
|
|
self.agent.current_task = self.agent._task_queue.get()
|
|||
|
|
priority, task = self.agent.current_task
|
|||
|
|
assert isinstance(task, Task)
|
|||
|
|
self.agent._planning_task(priority, task)
|
|||
|
|
task_executor.submit(task)
|
|||
|
|
|
|||
|
|
route = task.route
|
|||
|
|
for seg_index, segment in enumerate(task.route):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
while task.state in(TaskState.RUNNING, TaskState.PLANNED, TaskState.READY):
|
|||
|
|
|
|||
|
|
# 如果需要切换任务
|
|||
|
|
if self.agent.from_to(AgentState.RUNNING, AgentState.SWITCHING):
|
|||
|
|
self.agent._save_task(priority, task)
|
|||
|
|
task_executor.cancel(task.task_id)
|
|||
|
|
break
|
|||
|
|
# 如果需要停止任务
|
|||
|
|
elif self.agent.from_to(AgentState.RUNNING, AgentState.STOPPED):
|
|||
|
|
task_executor.cancel(task.task_id)
|
|||
|
|
break
|
|||
|
|
elif self.agent.from_to(AgentState.RUNNING, AgentState.PAUSED):
|
|||
|
|
task_executor.pause(task.task_id)
|
|||
|
|
elif self.agent.from_to(AgentState.PAUSED, AgentState.RUNNING):
|
|||
|
|
task_executor.resume(task.task_id)
|
|||
|
|
try:
|
|||
|
|
if task.state == TaskState.RUNNING and self.agent.state == AgentState.RUNNING:
|
|||
|
|
result = self.agent._model.step(self.dt)
|
|||
|
|
# 控制仿真速率（实时：sleep = dt；加速：sleep < dt）
|
|||
|
|
sleep(max(.050, (self.dt/1000) * 0.200))
|
|||
|
|
except Exception as e:
|
|||
|
|
traceback.print_exc()
|
|||
|
|
print(f"[SimThread] 仿真异常: {e}")
|
|||
|
|
break
|
|||
|
|
self.agent.state = AgentState.IDLE
|
|||
|
|
self.agent.current_task = None
|
|||
|
|
self.agent._model.stop()
|
|||
|
|
|
|||
|
|
class Agent(QObject):
|
|||
|
|
state_updated = pyqtSignal(object)
|
|||
|
|
|
|||
|
|
def __init__(
|
|||
|
|
self,
|
|||
|
|
controller: BaseUASController,
|
|||
|
|
parent: QObject = None,
|
|||
|
|
):
|
|||
|
|
super().__init__(parent)
|
|||
|
|
self._controller = controller
|
|||
|
|
# self._model = model
|
|||
|
|
# 日志
|
|||
|
|
self.logger = logging.getLogger(
|
|||
|
|
f"{self.__class__.__name__}[{self._model.equipment_id}]"
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
self._prev_state: object = AgentState.IDLE
|
|||
|
|
self._state: object = AgentState.IDLE
|
|||
|
|
|
|||
|
|
self._current_task: Optional[Tuple[int, Task]] = None
|
|||
|
|
self._task_lock = threading.Lock()
|
|||
|
|
self._task_queue = queue.PriorityQueue()
|
|||
|
|
self._task_thread = None
|
|||
|
|
|
|||
|
|
self._setup()
|
|||
|
|
|
|||
|
|
def _setup(self):
|
|||
|
|
# 注册事件类处理器
|
|||
|
|
event_bus.register_handler(DeviceErrorHandler())
|
|||
|
|
event_bus.register_handler(DeviceTimeoutHandler())
|
|||
|
|
event_bus.register_handler(EquipmentFaultHandler())
|
|||
|
|
# 扫描 @on_event 装饰器处理器
|
|||
|
|
count = event_bus.scan_decorators()
|
|||
|
|
event_bus.event_submitted.connect(self._on_event_submitted)
|
|||
|
|
event_bus.event_handled.connect(self._on_event_handled)
|
|||
|
|
event_bus.event_failed.connect(self._on_event_failed)
|
|||
|
|
event_bus.event_ignored.connect(self._on_event_ignored)
|
|||
|
|
event_bus.stats_updated.connect(self._on_stats_updated)
|
|||
|
|
|
|||
|
|
# ── 连接 ActionExecutor 信号 ──────────────────────────────
|
|||
|
|
action_executor.action_completed.connect(self._on_action_completed)
|
|||
|
|
action_executor.action_progress.connect(self._on_action_progress)
|
|||
|
|
action_executor.events_produced.connect(self._on_events_produced)
|
|||
|
|
|
|||
|
|
# ── 连接 TaskExecutor 信号 ────────────────────────────────
|
|||
|
|
task_executor.task_submitted.connect(self._on_task_submitted)
|
|||
|
|
task_executor.task_started.connect(self._on_task_started)
|
|||
|
|
task_executor.task_completed.connect(self._on_task_completed)
|
|||
|
|
task_executor.task_failed.connect(self._on_task_failed)
|
|||
|
|
task_executor.task_cancelled.connect(self._on_task_cancelled)
|
|||
|
|
task_executor.task_paused.connect(self._on_task_paused)
|
|||
|
|
task_executor.task_resumed.connect(self._on_task_resumed)
|
|||
|
|
task_executor.segment_entered.connect(self._on_segment_entered)
|
|||
|
|
task_executor.segment_exited.connect(self._on_segment_exited)
|
|||
|
|
action_executor.set_event_bus(event_bus)
|
|||
|
|
task_executor.set_action_executor(action_executor)
|
|||
|
|
|
|||
|
|
def _state_trans_handler(self):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
def put_task(self, priority: int, task: Task):
|
|||
|
|
with self._task_lock:
|
|||
|
|
self._task_queue.put((priority, task))
|
|||
|
|
if self._state == AgentState.IDLE:
|
|||
|
|
self._state = AgentState.READY
|
|||
|
|
self.state_updated.emit(self._state)
|
|||
|
|
|
|||
|
|
def remove_task(self, task: Optional[Task|str]):
|
|||
|
|
with self._task_lock:
|
|||
|
|
temp_queue = queue.PriorityQueue()
|
|||
|
|
while not self._task_queue.empty():
|
|||
|
|
priorty, _task = self._task_queue.get()
|
|||
|
|
assert isinstance(_task, Task)
|
|||
|
|
if isinstance(task, Task):
|
|||
|
|
if _task != task:
|
|||
|
|
temp_queue.put((priorty, task))
|
|||
|
|
elif isinstance(task, str):
|
|||
|
|
if _task.task_id != task:
|
|||
|
|
temp_queue.put((priorty, _task))
|
|||
|
|
self._task_queue = temp_queue
|
|||
|
|
if self._task_queue.empty() and self._state == AgentState.READY:
|
|||
|
|
self._state = AgentState.IDLE
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def state(self):
|
|||
|
|
with self._task_lock:
|
|||
|
|
return self._state
|
|||
|
|
|
|||
|
|
@state.setter
|
|||
|
|
def state(self, state: AgentState):
|
|||
|
|
with self._task_lock:
|
|||
|
|
self._prev_state = self._state
|
|||
|
|
self._state = state
|
|||
|
|
self.state_updated.emit(state)
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def current_task(self):
|
|||
|
|
with self._task_lock:
|
|||
|
|
return self._current_task
|
|||
|
|
|
|||
|
|
@current_task.setter
|
|||
|
|
def current_task(self, task):
|
|||
|
|
with self._task_lock:
|
|||
|
|
self._current_task = task
|
|||
|
|
|
|||
|
|
def schedule_task(self):
|
|||
|
|
self.state = AgentState.SWITCHING
|
|||
|
|
|
|||
|
|
def stop(self):
|
|||
|
|
self.state = AgentState.STOPPED
|
|||
|
|
|
|||
|
|
def pause(self):
|
|||
|
|
self.state = AgentState.PAUSED
|
|||
|
|
|
|||
|
|
def resume(self):
|
|||
|
|
self.state = AgentState.RUNNING
|
|||
|
|
|
|||
|
|
def from_to(self, from_state, to_state):
|
|||
|
|
with self._task_lock:
|
|||
|
|
if self._prev_state == from_state and self._state == to_state:
|
|||
|
|
return True
|
|||
|
|
else:
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def _process_task_queue(self, dt: float):
|
|||
|
|
if self.state == AgentState.RUNNING:
|
|||
|
|
return
|
|||
|
|
self._model.start()
|
|||
|
|
self.state = AgentState.RUNNING
|
|||
|
|
while self.state != AgentState.STOPPED and not self._task_queue.empty():
|
|||
|
|
self.current_task = self._task_queue.get()
|
|||
|
|
priority, task = self.current_task
|
|||
|
|
assert isinstance(task, Task)
|
|||
|
|
self._planning_task(priority, task)
|
|||
|
|
task_executor.submit(task)
|
|||
|
|
|
|||
|
|
while task.state in(TaskState.RUNNING, TaskState.PLANNED, TaskState.READY):
|
|||
|
|
# 如果需要切换任务
|
|||
|
|
if self.from_to(AgentState.RUNNING, AgentState.SWITCHING):
|
|||
|
|
self._save_task(priority, task)
|
|||
|
|
task_executor.cancel(task.task_id)
|
|||
|
|
break
|
|||
|
|
# 如果需要停止任务
|
|||
|
|
elif self.from_to(AgentState.RUNNING, AgentState.STOPPED):
|
|||
|
|
task_executor.cancel(task.task_id)
|
|||
|
|
break
|
|||
|
|
elif self.from_to(AgentState.RUNNING, AgentState.PAUSED):
|
|||
|
|
task_executor.pause(task.task_id)
|
|||
|
|
elif self.from_to(AgentState.PAUSED, AgentState.RUNNING):
|
|||
|
|
task_executor.resume(task.task_id)
|
|||
|
|
try:
|
|||
|
|
if task.state == TaskState.RUNNING and self.state == AgentState.RUNNING:
|
|||
|
|
result = self._model.step(dt)
|
|||
|
|
# 控制仿真速率（实时：sleep = dt；加速：sleep < dt）
|
|||
|
|
sleep(max(.050, (dt/1000) * 0.200))
|
|||
|
|
except Exception as e:
|
|||
|
|
traceback.print_exc()
|
|||
|
|
print(f"[SimThread] 仿真异常: {e}")
|
|||
|
|
break
|
|||
|
|
self.state = AgentState.IDLE
|
|||
|
|
self.current_task = None
|
|||
|
|
self._model.stop()
|
|||
|
|
|
|||
|
|
def start(self, dt):
|
|||
|
|
if self.state == AgentState.RUNNING:
|
|||
|
|
return
|
|||
|
|
self._task_thread = QThread()
|
|||
|
|
self._worker = AgentTaskWorker(self, dt)
|
|||
|
|
self._worker.moveToThread(self._task_thread)
|
|||
|
|
self._task_thread.started.connect(self._worker.do_work)
|
|||
|
|
self._worker.finished.connect(self._task_thread.quit)
|
|||
|
|
self._worker.finished.connect(self._task_thread.quit) # 任务结束 → 退出线程
|
|||
|
|
self._worker.finished.connect(self._worker.deleteLater) # 自动释放
|
|||
|
|
self._task_thread.finished.connect(self._task_thread.deleteLater) # 自动释放
|
|||
|
|
self._task_thread.finished.connect(self._on_worker_finished) # 任务结束后恢复按钮
|
|||
|
|
self._task_thread.start()
|
|||
|
|
|
|||
|
|
@pyqtSlot()
|
|||
|
|
def _on_worker_finished(self):
|
|||
|
|
self._task_thread = None
|
|||
|
|
|
|||
|
|
# ── 属性（只读）──────────────────────────────────────────────
|
|||
|
|
def _planning_task(self, priority, task):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
def _log(self, message, style):
|
|||
|
|
print(message)
|
|||
|
|
|
|||
|
|
def _save_task(self, priority, task):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_event_submitted(self, event):
|
|||
|
|
self._log(f"📥 提交 | {event.type_key} | src={event.source}", "#569cd6")
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_event_handled(self, event):
|
|||
|
|
results_str = " | ".join(str(r) for r in event.results)
|
|||
|
|
self._log(
|
|||
|
|
f"✅ 完成 | {event.type_key} | {event.elapsed_ms:.1f}ms | {results_str}",
|
|||
|
|
"#4ec9b0"
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
@pyqtSlot(object, str)
|
|||
|
|
def _on_event_failed(self, event, error):
|
|||
|
|
self._log(f"❌ 失败 | {event.type_key} | {error}", "#f44747")
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_event_ignored(self, event):
|
|||
|
|
self._log(f"⚪ 忽略 | {event.type_key} | 无处理器", "#808080")
|
|||
|
|
|
|||
|
|
@pyqtSlot(int, int, int)
|
|||
|
|
def _on_stats_updated(self, total: int, handled: int, failed: int):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ── TaskExecutor 信号槽 ───────────────────────────────────────
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_task_submitted(self, task):
|
|||
|
|
self._log(
|
|||
|
|
f"📥 任务提交: {task.name} [{task.task_id[:8]}]", "#569cd6"
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_task_started(self, task):
|
|||
|
|
self._log(f"▶ 任务开始执行: {task.name}", "#4ec9b0")
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_task_completed(self, task):
|
|||
|
|
self._log(
|
|||
|
|
f"✅ 任务完成: {task.name} 耗时={task.elapsed_s:.1f}s",
|
|||
|
|
"#4CAF50"
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
@pyqtSlot(object, str)
|
|||
|
|
def _on_task_failed(self, task, error):
|
|||
|
|
self._log(f"❌ 任务失败: {task.name} {error}", "#f44747")
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_task_cancelled(self, task):
|
|||
|
|
self._log(f"🚫 任务取消: {task.name}", "#FF9800")
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_task_paused(self, task):
|
|||
|
|
self._log(f"⏸ 任务暂停: {task.name}", "#9E9E9E")
|
|||
|
|
# self._add_rec_row("state_change", "任务已暂停", "")
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_task_resumed(self, task):
|
|||
|
|
self._log(f"▶ 任务恢复: {task.name}", "#4ec9b0")
|
|||
|
|
# self._add_rec_row("state_change", "任务已恢复", "")
|
|||
|
|
|
|||
|
|
@pyqtSlot(object, int)
|
|||
|
|
def _on_segment_entered(self, task, seg_index):
|
|||
|
|
if task.has_route and seg_index < len(task.route.segments):
|
|||
|
|
seg = task.route.segments[seg_index]
|
|||
|
|
msg = (f"进入航段[{seg_index}] "
|
|||
|
|
f"{seg.wp_from.identifier}→{seg.wp_to.identifier} "
|
|||
|
|
f"({seg.distance / 1000:.1f}km)")
|
|||
|
|
else:
|
|||
|
|
msg = f"进入航段[{seg_index}]"
|
|||
|
|
self._log(f"✈️ {msg}", "#dcdcaa")
|
|||
|
|
# self._add_rec_row("segment", msg, f"seg_index={seg_index}")
|
|||
|
|
|
|||
|
|
@pyqtSlot(object, int)
|
|||
|
|
def _on_segment_exited(self, task, seg_index):
|
|||
|
|
self._log(f"🏁 离开航段[{seg_index}]", "#808080")
|
|||
|
|
|
|||
|
|
# ── ActionExecutor 信号槽 ─────────────────────────────────────
|
|||
|
|
|
|||
|
|
@pyqtSlot(object)
|
|||
|
|
def _on_action_completed(self, action):
|
|||
|
|
self._log(
|
|||
|
|
f" ✅ Action完成: {action.type_key} "
|
|||
|
|
f"src={action.source} {action.elapsed_ms:.0f}ms",
|
|||
|
|
"#6a9955",
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
@pyqtSlot(str, int, str)
|
|||
|
|
def _on_action_progress(self, action_id: str, percent: int, message: str):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
@pyqtSlot(object, list)
|
|||
|
|
def _on_events_produced(self, action, events):
|
|||
|
|
# self._event_count += len(events)
|
|||
|
|
for ev in events:
|
|||
|
|
self._log(
|
|||
|
|
f" ⚡ Event产生: {ev.type_key} src={ev.source}",
|
|||
|
|
"#c586c0",
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
def __repr__(self) -> str:
|
|||
|
|
return (f"{self.__class__.__name__}("
|
|||
|
|
f"id={self.equipment_id}, "
|
|||
|
|
f"name={self.name}, "
|
|||
|
|
f"lc={self.lifecycle.value})")
|
|||
|
|
|