FaulknerWu/smart-inspection
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

任务执行

Browse Source
This commit is contained in:
ywb
2026-05-24 18:26:10 +08:00
parent d095d68433
commit 119246ade3
7 changed files with 338 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/utils/mission_executor.py
+132 -17
View File
@@ -41,6 +41,8 @@ class MissionStatus(str, Enum):
PAUSED = "paused"
COMPLETED = "completed"
WAITING_QR = "waiting_qr"
WAITING_ERROR = "waiting_error"
WAITING_STEP = "waiting_step"
class MissionExecutorV3:
@@ -70,6 +72,22 @@ class MissionExecutorV3:
self._qr_event = threading.Event()
self._qr_value: Optional[str] = None
# 错误弹窗
self._error_choice = None # "skip" or "abort"
# 单步执行
self._single_step_mode = False
self._step_choice = None # "confirm", "retry", "abort"
self._error_mode = False # True when waiting for error resolution
# 错误弹窗
self._error_choice = None # "skip" or "abort"
# 单步执行
self._single_step_mode = False
self._step_choice = None # "confirm", "retry", "abort"
self._error_mode = False # True when waiting for error resolution
# 设备
from .arm_client import ArmClient
self.arm_client = ArmClient(
@@ -104,6 +122,7 @@ class MissionExecutorV3:
machines: list,
qr_configs: list,
models: list,
single_step: bool = False,
) -> dict:
"""
执行完整拍摄任务。
@@ -167,25 +186,26 @@ class MissionExecutorV3:
"photos_back": 0,
} for (r, c) in path]
# 初始化任务列表
self.report["tasks"] = [{
"row": r, "col": c,
"machine_id": f"m_{r}_{c}",
"label": f"{r+1}-{c+1}",
"status": "pending",
"step": "等待",
"qr_value": None,
"photos_front": 0,
"photos_back": 0,
} for (r, c) in path]
# 机械臂初始姿态（AGV 移动前恢复）
arm_initial_pose = mission_config.get("arm_initial_pose", [0.0] * 6)
has_arm_pose = self.arm_client and any(abs(a) > 0.01 for a in arm_initial_pose)
# 2. 逐台执行
for idx, (r, c) in enumerate(path):
machine_idx = 0
while machine_idx < len(path):
if self._stop.is_set():
self._log("⏹️ 任务已停止")
break
self._wait_pause()
r, c = path[machine_idx]
rl, cl = r + 1, c + 1
# 恢复机械臂初始姿态
if has_arm_pose:
self._log(" 🦾 恢复机械臂初始姿态")
self.arm_client.set_angles(arm_initial_pose, speed=500)
time.sleep(2)
# 更新任务状态 → 正面开始
task = self._get_task(r, c)
@@ -196,11 +216,10 @@ class MissionExecutorV3:
machine_id = f"m_{r}_{c}"
machine = next((m for m in machines if m.get("id") == machine_id), None)
if not machine:
self._log(f"⚠️ 机器 {r+1}-{c+1} 不存在，跳过")
self._log(f"⚠️ 机器 {rl}-{cl} 不存在，跳过")
machine_idx += 1
continue
rl, cl = r + 1, c + 1 # 显示用的 1-based
# --- 正面 ---
self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
self._step(f"机器 {rl}-{cl} 正面")
@@ -210,6 +229,9 @@ class MissionExecutorV3:
if front_pt and self._has_coords(front_pt):
if not self._navigate(front_pt, "正面"):
self._log(f"⚠️ 导航失败，尝试继续")
choice = self._wait_error(f"机器 {rl}-{cl} 正面导航失败")
if choice == "abort":
break
else:
self._log(f"⚠️ 无正面点位坐标")
@@ -233,7 +255,7 @@ class MissionExecutorV3:
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
self._progress(idx, 1)
self._progress(machine_idx, 1)
# --- 背面 ---
if task:
@@ -246,6 +268,9 @@ class MissionExecutorV3:
if back_pt and self._has_coords(back_pt):
if not self._navigate(back_pt, "背面"):
self._log(f"⚠️ 导航失败，尝试继续")
choice = self._wait_error(f"机器 {rl}-{cl} 背面导航失败")
if choice == "abort":
break
else:
self._log(f"⚠️ 无背面点位坐标")
@@ -257,7 +282,21 @@ class MissionExecutorV3:
if task:
task["status"] = "completed"
task["step"] = "完成"
self._progress(idx, 2)
self._progress(machine_idx, 2)
# 单步执行：等待用户确认
if single_step and not self._stop.is_set():
choice = self._wait_step_confirm(rl, cl)
if choice == "abort":
break
elif choice == "retry":
if task:
task["status"] = "pending"
task["step"] = "重试开始"
self._progress(machine_idx, 0)
continue # 不递增 machine_idx，重新执行
machine_idx += 1
# 3. 回到出发点
if not self._stop.is_set():
@@ -607,6 +646,82 @@ class MissionExecutorV3:
99
)
# ==================== 错误弹窗 ====================
def _wait_error(self, msg: str) -> str:
"""阻塞等待用户选择：skip（跳过）或 abort（中断）"""
self.status = MissionStatus.WAITING_ERROR
self.report["status"] = "waiting_error"
self.report["step"] = msg
self.report["error"] = msg
self._log(f"⚠️ 错误处理: {msg}")
self._error_choice = None
self._error_mode = True
start = time.time()
while self._error_choice is None and not self._stop.is_set():
time.sleep(0.2)
if time.time() - start > 600: # 10分钟超时 → 跳过
self._error_choice = "skip"
choice = self._error_choice or "skip"
self._error_choice = None
self._error_mode = False
if choice == "abort":
self._stop.set()
self._log("⏹️ 用户选择中断")
else:
self._log("⏭️ 用户选择跳过")
# 恢复状态
self.status = MissionStatus.RUNNING if not self._single_step_mode else MissionStatus.WAITING_STEP
self.report["status"] = self.status.value
self.report["error"] = None
return choice
def set_error_choice(self, choice: str):
"""外部 API 设置错误处理选择"""
self._error_choice = choice
# ==================== 单步执行 ====================
def _wait_step_confirm(self, row_label: int, col_label: int) -> str:
"""单步执行：等待用户确认/重试/中断"""
self.status = MissionStatus.WAITING_STEP
self.report["status"] = "waiting_step"
self.report["step"] = f"机器 {row_label}-{col_label} 完成，等待确认"
self.report["current_step"] = {
"row": row_label - 1, "col": col_label - 1,
"label": f"{row_label}-{col_label}"
}
self._log(f"⏸️ 单步执行: 机器 {row_label}-{col_label} 完成，等待确认...")
self._step_choice = None
start = time.time()
while self._step_choice is None and not self._stop.is_set():
time.sleep(0.2)
if time.time() - start > 600: # 10分钟超时 → 确认
self._step_choice = "confirm"
choice = self._step_choice or "confirm"
self._step_choice = None
self.report.pop("current_step", None)
if choice == "abort":
self._stop.set()
self._log("⏹️ 用户选择中断")
elif choice == "retry":
self._log(f"🔄 用户选择重试机器 {row_label}-{col_label}")
else:
self._log(f"✅ 用户确认机器 {row_label}-{col_label}")
return choice
def set_step_choice(self, choice: str):
"""外部 API 设置单步执行选择"""
self._step_choice = choice
# ==================== Nav2 导航 ====================
# (保留原实现)