# -*- 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})")