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-06-01 19:11:17 +08:00
parent e7c9621c65
commit 0b5862b271
2 changed files with 146 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/utils/mission_executor.py
+110 -67
View File
@@ -297,17 +297,18 @@ class MissionExecutorV3:
cl_back = c + 1 if has_back else 0
log_parts = []
if has_back:
log_parts.append(f"背面:机器{rl_back}-{cl_back}")
task = self._get_task(pr - 1, c)
if task:
task["status"] = "active"
task["step"] = "背面拍照"
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})")
@@ -347,55 +348,8 @@ class MissionExecutorV3:
if pk in self.report.get("point_status", {}):
self.report["point_status"][pk] = "done"
# --- 正面操作（机器 pr,c 的正面） ---
qr_value = None
if has_front and not self._stop.is_set():
self._wait_pause()
# 更新机器状态：正面开始
mk = f"{pr}_{c}"
if mk in self.report.get("machine_status", {}):
self.report["machine_status"][mk]["status"] = "active"
self.report["machine_status"][mk]["step"] = "正面扫码"
if opt_qr_scan:
qr_value = self._scan_qr_with_poses(qr_configs, machine_row=pr)
if self._stop.is_set():
break
else:
self._log(" ⏭️ 跳过二维码识别（正面）")
qr_cache[(pr, c)] = qr_value
# 更新机器状态：扫码完成
mk2 = f"{pr}_{c}"
if mk2 in self.report.get("machine_status", {}):
self.report["machine_status"][mk2]["qr"] = "done" if qr_value else "skipped"
self.report["machine_status"][mk2]["qr_val"] = qr_value
self.report["machine_status"][mk2]["step"] = "正面拍照"
task = self._get_task(pr, c)
if task and qr_value:
task["qr_value"] = qr_value
if task:
task["step"] = "正面拍照"
model_name = self._lookup_model(qr_value)
self._log(f" 🏷️ 机型: {model_name}")
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", pr)
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
else:
self._log(" ⏭️ 跳过正面拍照")
completed_actions += 1
# 更新机器状态：正面拍照完成
mk3 = f"{pr}_{c}"
if mk3 in self.report.get("machine_status", {}):
self.report["machine_status"][mk3]["front"] = "done" if opt_front_photo else "skipped"
self.report["machine_status"][mk3]["front_cnt"] = self.report["machine_status"][mk3].get("front_cnt", 0) + 1
# --- 背面操作（机器 pr-1,c 的背面） ---
# 同一点位同时服务上一行背面和下一行正面时，必须先完成上一行背面。
if has_back and not self._stop.is_set():
self._wait_pause()
back_qr = qr_cache.get((pr - 1, c), "unknown")
@@ -431,6 +385,53 @@ class MissionExecutorV3:
task["status"] = "completed"
task["step"] = "完成"
# --- 正面操作（机器 pr,c 的正面） ---
qr_value = None
if has_front and not self._stop.is_set():
self._wait_pause()
# 更新机器状态：正面开始
mk = f"{pr}_{c}"
if mk in self.report.get("machine_status", {}):
self.report["machine_status"][mk]["status"] = "active"
self.report["machine_status"][mk]["step"] = "正面扫码"
if opt_qr_scan:
qr_value = self._scan_qr_with_poses(qr_configs, machine_row=pr)
if self._stop.is_set():
break
else:
self._log(" ⏭️ 跳过二维码识别（正面）")
qr_cache[(pr, c)] = qr_value
# 更新机器状态：扫码完成
mk2 = f"{pr}_{c}"
if mk2 in self.report.get("machine_status", {}):
self.report["machine_status"][mk2]["qr"] = "done" if qr_value else "skipped"
self.report["machine_status"][mk2]["qr_val"] = qr_value
self.report["machine_status"][mk2]["step"] = "正面拍照"
task = self._get_task(pr, c)
if task and qr_value:
task["qr_value"] = qr_value
if task:
task["step"] = "正面拍照"
model_name = self._lookup_model(qr_value)
self._log(f" 🏷️ 机型: {model_name}")
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", pr)
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
else:
self._log(" ⏭️ 跳过正面拍照")
completed_actions += 1
# 更新机器状态：正面拍照完成
mk3 = f"{pr}_{c}"
if mk3 in self.report.get("machine_status", {}):
self.report["machine_status"][mk3]["front"] = "done" if opt_front_photo else "skipped"
self.report["machine_status"][mk3]["front_cnt"] = self.report["machine_status"][mk3].get("front_cnt", 0) + 1
# 更新进度
if max_actions:
self.report["progress"] = min(int(completed_actions / max_actions * 100), 99)
@@ -456,20 +457,20 @@ class MissionExecutorV3:
# 3. 回到出发点（必须成功返回才算结束）
if not self._stop.is_set() and opt_agv_move:
if opt_arm_init and has_arm_pose:
self._step("恢复机械臂初始姿态")
self._log(" 🦾 返回前恢复机械臂初始姿态")
try:
ok = self.arm_client.set_angles(arm_initial_pose, speed=self.arm_speed)
if ok:
self._wait_arm_ready(arm_initial_pose)
self._log(" ✅ 机械臂已恢复初始姿态")
else:
self._log(" ⚠️ 机械臂初始姿态指令发送失败，继续尝试返回原点")
except Exception as e:
self._log(f" ⚠️ 返回前机械臂初始化失败: {e}")
self._step("返回出发点")
max_retry = 3
for attempt in range(1, max_retry + 1):
self._log(f"→ 返回 (0, 0) (尝试 {attempt}/{max_retry})")
ok = self._nav2_go_to_point(0, 0, 0, timeout_sec=180)
if ok:
self._log("✅ 已返回出发点")
break
else:
self._log(f"⚠️ 返回失败 (尝试 {attempt}/{max_retry})")
if attempt < max_retry:
self._log("⏳ 等待 3 秒后重试...")
time.sleep(3)
else:
if not self._return_to_origin():
self._log("❌ 返回出发点失败（已达最大重试次数），任务标记为异常")
elif not self._stop.is_set():
self._log("⏭️ 跳过返回出发点")
@@ -619,6 +620,43 @@ class MissionExecutorV3:
self._log(f" 🧭 导航到{label}点位 ({x:.2f}, {y:.2f}, yaw={math.degrees(yaw):.0f}°)")
return self._nav2_go_to_point(x, y, yaw)
def _return_to_origin(self) -> bool:
"""返回原点。
常规任务使用阻塞导航等待到达；如果旧 navigator 状态异常，再用新的
Nav2Navigator 重试。最后兜底使用与设置页“回到原点”一致的非阻塞发送。
"""
max_retry = 3
for attempt in range(1, max_retry + 1):
self._log(f"→ 返回 (0, 0) (尝试 {attempt}/{max_retry})")
navigator = self._nav if attempt == 1 else Nav2Navigator()
ok = self._nav2_go_to_point_with(navigator, 0, 0, 0, timeout_sec=240)
if ok:
if attempt > 1:
self._nav = navigator
self._log("✅ 已返回出发点")
return True
self._log(f"⚠️ 返回失败 (尝试 {attempt}/{max_retry})")
if attempt < max_retry:
self._log("⏳ 等待 3 秒后重试...")
time.sleep(3)
try:
self._log("↪️ 调用设置页同款接口返回原点")
resp = requests.post(
"http://127.0.0.1:5000/api/navigate/to",
json={"x": 0, "y": 0, "yaw": 0},
timeout=8,
)
data = resp.json() if resp.content else {}
if resp.status_code == 200 and data.get("ok"):
self._log("✅ 已通过 /api/navigate/to 发送返回出发点导航")
return True
self._log(f"⚠️ /api/navigate/to 返回失败: {data.get('error') or resp.text}")
except Exception as e:
self._log(f"⚠️ 调用 /api/navigate/to 返回原点失败: {e}")
return False
# ==================== 二维码扫描 ====================
@@ -1005,9 +1043,14 @@ class MissionExecutorV3:
def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
timeout_sec: float = 120.0) -> bool:
"""使用 Nav2Navigator 直接发送导航目标（blocking 模式，等待完成）"""
return self._nav2_go_to_point_with(self._nav, x, y, yaw, timeout_sec)
def _nav2_go_to_point_with(self, navigator: Nav2Navigator, x: float, y: float,
yaw: float = 0.0, timeout_sec: float = 120.0) -> bool:
"""使用指定 Nav2Navigator 发送导航目标（blocking 模式，等待完成）"""
try:
logger.info(f"🧭 导航到目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
ok = self._nav.navigate_to_pose(x, y, yaw, timeout_sec=timeout_sec, blocking=True)
ok = navigator.navigate_to_pose(x, y, yaw, timeout_sec=timeout_sec, blocking=True)
if ok:
logger.info(f"✅ 导航成功到达 ({x:.3f}, {y:.3f})")
else: