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/app.py
+26 -50
View File
@@ -894,77 +894,53 @@ def api_mission_poses_delete(machine_id, side, pose_id):
@app.route("/api/mission/generate_sequence", methods=["GET"])
def api_mission_generate_sequence():
"""根据网格配置和机器配置生成拍摄序列（蛇形）"""
rows = gs.mission_config.get("rows", 2)
cols = gs.mission_config.get("cols", 3)
rows = int(gs.mission_config.get("rows", 2))
cols = int(gs.mission_config.get("cols", 3))
grid = gs.mission_config.get("grid", [])
machines = gs.machines_config
if (not grid or all(not any(row) if isinstance(row, list) else True for row in grid)) and machines:
grid = [[False] * cols for _ in range(rows)]
for m in machines:
r = int(m.get("row", 0))
c = int(m.get("col", 0))
if 0 <= r < rows and 0 <= c < cols:
grid[r][c] = True
def get_machine(row, col):
for m in machines:
if m.get("row") == row and m.get("col") == col:
return m
return None
# 蛇形序列：行0从左到右正面→右到左背面，行1从右到左背面→左到右正面...交替
# 点位蛇形序列：同一点位同时有上一行背面和下一行正面时，先背面再正面。
sequence = []
for r in range(rows):
# 检查该行是否有机器
has_any = any(grid[r][c] for c in range(cols)) if r < len(grid) else False
if not has_any:
continue
if r % 2 == 0: # 偶数行：正面从左到右，背面从右到左
# 正面：从左到右
for c in range(cols):
if r < len(grid) and c < len(grid[r]) and grid[r][c]:
m = get_machine(r, c)
if m and m.get("front"):
sequence.append({
"machine_id": m["id"],
"row": r, "col": c,
"side": "front",
"row_dir": "lr", # 正面时该行的方向
"row_dir_back": "rl" # 背面时该行的方向
})
# 背面：从右到左
for c in range(cols - 1, -1, -1):
if r < len(grid) and c < len(grid[r]) and grid[r][c]:
m = get_machine(r, c)
for pr in range(rows + 1):
cols_iter = range(cols) if pr % 2 == 0 else range(cols - 1, -1, -1)
row_dir = "lr" if pr % 2 == 0 else "rl"
for c in cols_iter:
if pr > 0 and pr - 1 < len(grid) and c < len(grid[pr - 1]) and grid[pr - 1][c]:
m = get_machine(pr - 1, c)
if m and m.get("back"):
sequence.append({
"machine_id": m["id"],
"row": r, "col": c,
"row": pr - 1, "col": c,
"point_row": pr,
"side": "back",
"row_dir": "lr",
"row_dir_back": "rl"
"row_dir": row_dir
})
else: # 奇数行：正面从右到左，背面从左到右（方向反转）
# 背面：从左到右（此行的背面在下一行的前面位置，但这里按用户描述：背面先行）
for c in range(cols):
if r < len(grid) and c < len(grid[r]) and grid[r][c]:
m = get_machine(r, c)
if m and m.get("back"):
sequence.append({
"machine_id": m["id"],
"row": r, "col": c,
"side": "back",
"row_dir": "rl",
"row_dir_back": "lr"
})
# 正面：从右到左
for c in range(cols - 1, -1, -1):
if r < len(grid) and c < len(grid[r]) and grid[r][c]:
m = get_machine(r, c)
if pr < rows and pr < len(grid) and c < len(grid[pr]) and grid[pr][c]:
m = get_machine(pr, c)
if m and m.get("front"):
sequence.append({
"machine_id": m["id"],
"row": r, "col": c,
"row": pr, "col": c,
"point_row": pr,
"side": "front",
"row_dir": "rl",
"row_dir_back": "lr"
"row_dir": row_dir
})
return json
return jsonify({"ok": True, "sequence": sequence})
# ========== 点位配置 API（独立于机器）==========
@app.route("/api/mission/positions", methods=["GET"])
agv_app/utils/mission_executor.py
+109 -66
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:
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 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._log("✅ 已返回出发点")
break
else:
self._log(f"⚠️ 返回失败 (尝试 {attempt}/{max_retry})")
if attempt < max_retry:
self._log("⏳ 等待 3 秒后重试...")
time.sleep(3)
self._wait_arm_ready(arm_initial_pose)
self._log(" ✅ 机械臂已恢复初始姿态")
else:
self._log(" ⚠️ 机械臂初始姿态指令发送失败，继续尝试返回原点")
except Exception as e:
self._log(f" ⚠️ 返回前机械臂初始化失败: {e}")
self._step("返回出发点")
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: