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-28 13:55:55 +08:00
parent f507363c34
commit 4c096a4bd1
12 changed files with 1207 additions and 669 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/utils/mission_executor.py
+182 -126
View File
@@ -185,31 +185,43 @@ class MissionExecutorV3:
# 1. 生成蛇形路径
path = MissionExecutorV3._build_snake_path(rows, cols, grid)
if not path:
self._log("❌ 网格中没有机器，任务终止")
self.report["error"] = "No machines in grid"
self._log("❌ 网格中没有点位，任务终止")
self.report["error"] = "No points in grid"
return self._finish(0)
self.report["total"] = len(path)
self._log(f"📍 蛇形路径生成: {len(path)} 台机器")
# 统计总机器数（用于进度计算）
total_machines = 0
for rw in grid:
for v in rw:
if v:
total_machines += 1
self.report["total"] = total_machines
# 扫码结果缓存：正面扫到的 QR 传给背面
qr_cache = {}
# 初始化任务列表
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]
# 初始化任务列表（机器级别）
self.report["tasks"] = []
for r in range(rows):
for c in range(cols):
if MissionExecutorV3._has_machine(grid, r, c):
self.report["tasks"].append({
"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,
})
self._log(f"📍 点位蛇形路径: {len(path)} 个点位, {total_machines} 台机器")
# 任务步骤控制开关
if options is None:
options = {}
opt_arm_init = options.get("arm_init", True)
opt_agv_move = options.get("agv_move", True)
# 机械臂初始化并入 AGV 移动：只要开移动，就先初始化机械臂
if opt_agv_move:
opt_arm_init = True
opt_qr_scan = options.get("qr_scan", True)
@@ -222,140 +234,151 @@ class MissionExecutorV3:
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. 逐台执行
machine_idx = 0
while machine_idx < len(path):
# 进度统计
max_actions = total_machines * 2 # 每台机器正面+背面
completed_actions = 0
# 2. 逐点位蛇形执行
pi = 0
while pi < len(path):
if self._stop.is_set():
self._log("⏹️ 任务已停止")
break
self._wait_pause()
r, c = path[machine_idx]
rl, cl = r + 1, c + 1
pr, c = path[pi] # pr = 点位行, c = 列
# 恢复机械臂初始姿态
if opt_arm_init and has_arm_pose:
# 判断该点位需要做什么
has_front = pr < rows and MissionExecutorV3._has_machine(grid, pr, c)
has_back = pr > 0 and MissionExecutorV3._has_machine(grid, pr - 1, c)
if not has_front and not has_back:
pi += 1
continue
# 日志 & 步骤更新
rl_front = pr + 1 if has_front else 0
cl_front = c + 1 if has_front else 0
rl_back = pr if has_back else 0
cl_back = c + 1 if has_back else 0
log_parts = []
if has_front:
log_parts.append(f"正面:机器{rl_front}-{cl_front}")
task = self._get_task(pr, c)
if task:
task["status"] = "active"
task["step"] = "正面扫码"
if has_back:
log_parts.append(f"背面:机器{rl_back}-{cl_back}")
task = self._get_task(pr - 1, c)
if task:
task["step"] = "背面拍照"
self._log(f"📍 点位 ({pr},{c}) → {' & '.join(log_parts)}")
self._step(f"点位({pr},{c})")
# 恢复机械臂初始姿态（AGV 移动前）
if opt_arm_init and has_arm_pose and opt_agv_move:
self._log(" 🦾 恢复机械臂初始姿态")
try:
self.arm_client.set_angles(arm_initial_pose, speed=500)
time.sleep(2)
self._wait_arm_ready(arm_initial_pose)
except Exception as e:
self._log(f" ⚠️ 机械臂初始化失败: {e}")
# 更新任务状态 → 正面开始
task = self._get_task(r, c)
if task:
task["status"] = "active"
task["step"] = "正面扫码"
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"⚠️ 机器 {rl}-{cl} 不存在，跳过")
machine_idx += 1
continue
# --- 正面 ---
self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
self._step(f"机器 {rl}-{cl} 正面")
# 导航到正面点位
# 导航到该点位的坐标
if opt_agv_move:
front_pt = self._find_point(positions, r, c, "front")
if front_pt and self._has_coords(front_pt):
if not self._navigate(front_pt, "正面"):
# 找该点位的任意有效坐标（正面/背面坐标相同）
pos = MissionExecutorV3._find_any_position(positions, pr, c)
if pos and MissionExecutorV3._has_coords(pos):
if not self._navigate(pos, f"点位({pr},{c})"):
self._log(f"⚠️ 导航失败，尝试继续")
choice = self._wait_error(f"机器 {rl}-{cl} 正面导航失败")
choice = self._wait_error(f"点位({pr},{c})导航失败")
if choice == "abort":
break
else:
self._log(f"⚠️ 无正面点位坐标")
self._log(f"⚠️ 点位({pr},{c})无有效坐标")
else:
self._log(" ⏭️ 跳过AGV移动（正面）")
self._log(" ⏭️ 跳过AGV移动")
# 扫描二维码
# --- 正面操作（机器 pr,c 的正面） ---
qr_value = None
if opt_qr_scan:
qr_value = self._scan_qr_with_poses(qr_configs)
if self._stop.is_set():
break
else:
self._log(" ⏭️ 跳过二维码识别")
# 查机型 + 更新任务步骤
model_name = self._lookup_model(qr_value)
self._log(f" 🏷️ 机型: {model_name}")
if task and qr_value:
task["qr_value"] = qr_value
if task:
task["step"] = "正面拍照"
# 正面拍照
if opt_front_photo:
model = self._find_model(models, model_name)
if model:
self._shoot(model, "front", rl, cl, qr_value or "unknown")
if has_front and not self._stop.is_set():
self._wait_pause()
if opt_qr_scan:
qr_value = self._scan_qr_with_poses(qr_configs)
if self._stop.is_set():
break
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
else:
self._log(" ⏭️ 跳过正面拍照")
self._log(" ⏭️ 跳过二维码识别（正面）")
qr_cache[(pr, c)] = qr_value
self._progress(machine_idx, 1)
task = self._get_task(pr, c)
if task and qr_value:
task["qr_value"] = qr_value
if task:
task["step"] = "正面拍照"
# --- 背面 ---
if task:
task["step"] = "背面拍照"
self._log(f"📍 机器 {rl}-{cl} 进入背面点位")
self._step(f"机器 {rl}-{cl} 背面")
model_name = self._lookup_model(qr_value)
self._log(f" 🏷️ 机型: {model_name}")
# 导航到背面点位
# 导航到背面点位之前：恢复机械臂初始姿态
if opt_agv_move and has_arm_pose:
self._log(" 🦾 恢复机械臂初始姿态（背面移动前）")
try:
self.arm_client.set_angles(arm_initial_pose, speed=500)
time.sleep(2)
except Exception as e:
self._log(f" ⚠️ 机械臂初始化失败: {e}")
if opt_agv_move:
back_pt = self._find_point(positions, r + 1, c, "back")
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
if opt_front_photo and not self._stop.is_set():
model = self._find_model(models, model_name)
if model:
self._shoot(model, "front", rl_front, cl_front, qr_value or "unknown")
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
else:
self._log(f"⚠️ 无背面点位坐标")
else:
self._log(" ⏭️ 跳过AGV移动（背面）")
self._log(" ⏭️ 跳过正面拍照")
completed_actions += 1
# 背面拍照
if opt_back_photo:
if model:
self._shoot(model, "back", rl, cl, qr_value or "unknown")
else:
self._log(" ⏭️ 跳过背面拍照")
# --- 背面操作（机器 pr-1,c 的背面） ---
if has_back and not self._stop.is_set():
self._wait_pause()
back_qr = qr_cache.get((pr - 1, c), "unknown")
# 标记任务完成
if task:
task["status"] = "completed"
task["step"] = "完成"
self._progress(machine_idx, 2)
task = self._get_task(pr - 1, c)
if task:
task["step"] = "背面拍照"
# 单步执行：等待用户确认
model_name = self._lookup_model(back_qr)
self._log(f" 🏷️ 机型(背面): {model_name}")
if opt_back_photo and not self._stop.is_set():
model = self._find_model(models, model_name)
if model:
self._shoot(model, "back", rl_back, cl_back, back_qr)
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
else:
self._log(" ⏭️ 跳过背面拍照")
completed_actions += 1
if task:
task["status"] = "completed"
task["step"] = "完成"
# 更新进度
if max_actions:
self.report["progress"] = min(int(completed_actions / max_actions * 100), 99)
# 单步执行
if single_step and not self._stop.is_set():
choice = self._wait_step_confirm(rl, cl)
choice = self._wait_step_confirm(
rl_front if has_front else rl_back,
cl_front if has_front else cl_back
)
if choice == "abort":
break
elif choice == "retry":
if task:
task["status"] = "pending"
task["step"] = "重试开始"
self._progress(machine_idx, 0)
continue # 不递增 machine_idx，重新执行
if has_front:
task = self._get_task(pr, c)
if task:
task["status"] = "pending"
task["step"] = "重试开始"
completed_actions = max(0, completed_actions - 2)
continue
machine_idx += 1
pi += 1
# 3. 回到出发点
if not self._stop.is_set() and opt_agv_move:
@@ -391,17 +414,22 @@ class MissionExecutorV3:
@staticmethod
def _build_snake_path(rows: int, cols: int, grid: list) -> list:
"""奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左"""
"""生成点位级蛇形路径：遍历点位行 0→rows，奇数行左→右，偶数行右→左
每个点位 (pr, c) 同时服务：
- 正面：机器 (pr, c)（如果 pr < rows 且 grid[pr][c] 为真）
- 背面：机器 (pr-1, c)（如果 pr > 0 且 grid[pr-1][c] 为真）
按此路径走完所有点位，是最短的蛇形走位，不再反复横跳。
"""
path = []
for r in range(rows):
if r % 2 == 0:
for pr in range(rows + 1): # 点位行 = 机器行 + 1
if pr % 2 == 0:
for c in range(cols):
if MissionExecutorV3._has_machine(grid, r, c):
path.append((r, c))
path.append((pr, c))
else:
for c in range(cols - 1, -1, -1):
if MissionExecutorV3._has_machine(grid, r, c):
path.append((r, c))
path.append((pr, c))
return path
@staticmethod
@@ -461,6 +489,15 @@ class MissionExecutorV3:
# ==================== 点位查找 ====================
@staticmethod
def _find_any_position(positions: list, row: int, col: int) -> Optional[dict]:
"""查找点位的任意有效坐标（正面/背面坐标相同，取第一个有坐标的）"""
for side in ("front", "back"):
p = MissionExecutorV3._find_point(positions, row, col, side)
if p and MissionExecutorV3._has_coords(p):
return p
return None
@staticmethod
def _find_point(positions: list, row: int, col: int, side: str) -> Optional[dict]:
for p in positions:
@@ -484,6 +521,25 @@ class MissionExecutorV3:
# ==================== 二维码扫描 ====================
def _wait_arm_ready(self, target_angles: list, timeout: float = 10.0, tolerance: float = 3.0) -> bool:
"""等待机械臂稳定到目标角度（±tolerance 度），超时返回 False"""
if not self.arm_client:
return True
deadline = time.time() + timeout
while time.time() < deadline:
try:
ok, current_rad = self.arm_client.get_angles()
if ok and current_rad and len(current_rad) >= 6:
# get_angles() 返回弧度，target_angles 是角度，需转换后比较
current_deg = [math.degrees(a) for a in current_rad]
if all(abs(current_deg[i] - target_angles[i]) < tolerance for i in range(6)):
return True
except Exception:
pass
time.sleep(0.5)
self._log(f" ⚠️ 机械臂稳定等待超时 (target={target_angles})")
return False
def _scan_qr_with_poses(self, qr_configs: list) -> Optional[str]:
"""用二维码配置中的姿态依次尝试，逐一调整姿态+等2秒+扫码，全部失败才弹框"""
if not qr_configs:
@@ -502,7 +558,7 @@ class MissionExecutorV3:
# 调整机械臂姿态
if self.arm_client:
self.arm_client.set_angles(angles, speed=500)
time.sleep(2) # 等机械臂稳定 + 摄像头图像稳定
self._wait_arm_ready(angles)
# 读取摄像头并扫码
qr = self._decode_qr_from_arm()
if qr:
@@ -601,7 +657,7 @@ class MissionExecutorV3:
# 调整机械臂
if self.arm_client:
self.arm_client.set_angles(angles, speed=500)
time.sleep(QR_POSE_WAIT)
self._wait_arm_ready(angles)
# 拍照
path = self._capture_arm_photo(row, col, side, pi + 1, qr_value)