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-27 14:49:40 +08:00
parent c1601c0a85
commit fcf5649d8c
5 changed files with 209 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/utils/mission_executor.py
+70 -27
View File
@@ -124,10 +124,22 @@ class MissionExecutorV3:
qr_configs: list,
models: list,
single_step: bool = False,
options: dict = None,
) -> dict:
"""
执行完整拍摄任务。
Args:
options: 任务步骤控制开关
- arm_init: 是否执行机械臂位置初始化
- agv_move: 是否执行AGV移动
- qr_scan: 是否执行二维码识别
- front_photo: 是否执行正面拍照
- back_photo: 是否执行背面拍照
"""
"""
执行完整拍摄任务。
Args:
mission_config: {rows, cols, grid, positions}
machines: [{id, row, col, front: {coords}, back: {coords}}]
@@ -187,6 +199,17 @@ class MissionExecutorV3:
"photos_back": 0,
} for (r, c) in path]
# 任务步骤控制开关
if options is None:
options = {}
opt_arm_init = options.get("arm_init", True)
opt_agv_move = options.get("agv_move", True)
opt_qr_scan = options.get("qr_scan", True)
opt_front_photo = options.get("front_photo", True)
opt_back_photo = options.get("back_photo", True)
self._log(f"⚙️ 任务步骤: arm_init={opt_arm_init}, agv_move={opt_agv_move}, "
f"qr_scan={opt_qr_scan}, front={opt_front_photo}, back={opt_back_photo}")
# 机械臂初始姿态（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)
@@ -203,10 +226,12 @@ class MissionExecutorV3:
rl, cl = r + 1, c + 1
# 恢复机械臂初始姿态
if has_arm_pose:
if opt_arm_init and has_arm_pose:
self._log(" 🦾 恢复机械臂初始姿态")
self.arm_client.set_angles(arm_initial_pose, speed=500)
time.sleep(2)
elif not opt_arm_init:
self._log(" ⏭️ 跳过机械臂位置初始化")
# 更新任务状态 → 正面开始
task = self._get_task(r, c)
@@ -226,20 +251,27 @@ class MissionExecutorV3:
self._step(f"机器 {rl}-{cl} 正面")
# 导航到正面点位
front_pt = self._find_point(positions, r, c, "front")
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
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, "正面"):
self._log(f"⚠️ 导航失败，尝试继续")
choice = self._wait_error(f"机器 {rl}-{cl} 正面导航失败")
if choice == "abort":
break
else:
self._log(f"⚠️ 无正面点位坐标")
else:
self._log(f"⚠️ 无正面点位坐标")
self._log(" ⏭️ 跳过AGV移动（正面）")
# 扫描二维码
qr_value = self._scan_qr_with_poses(qr_configs)
if self._stop.is_set():
break
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)
@@ -250,11 +282,14 @@ class MissionExecutorV3:
task["step"] = "正面拍照"
# 正面拍照
model = self._find_model(models, model_name)
if model:
self._shoot(model, "front", rl, cl, qr_value or "unknown")
if opt_front_photo:
model = self._find_model(models, model_name)
if model:
self._shoot(model, "front", rl, cl, qr_value or "unknown")
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
self._log(" ⏭️ 跳过正面拍照")
self._progress(machine_idx, 1)
@@ -265,19 +300,25 @@ class MissionExecutorV3:
self._step(f"机器 {rl}-{cl} 背面")
# 导航到背面点位
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_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
else:
self._log(f"⚠️ 无背面点位坐标")
else:
self._log(f"⚠️ 无背面点位坐标")
self._log(" ⏭️ 跳过AGV移动（背面）")
# 背面拍照
if model:
self._shoot(model, "back", rl, cl, qr_value or "unknown")
if opt_back_photo:
if model:
self._shoot(model, "back", rl, cl, qr_value or "unknown")
else:
self._log(" ⏭️ 跳过背面拍照")
# 标记任务完成
if task:
@@ -300,10 +341,12 @@ class MissionExecutorV3:
machine_idx += 1
# 3. 回到出发点
if not self._stop.is_set():
if not self._stop.is_set() and opt_agv_move:
self._step("返回出发点")
self._log("→ 返回 (0, 0)")
self._nav2_go_to_point(0, 0, 0, timeout_sec=60)
elif not self._stop.is_set():
self._log("⏭️ 跳过返回出发点")
elapsed = time.time() - start_time
return self._finish(elapsed)