diff --git a/agv_app/app.py b/agv_app/app.py index ea63817..3fb06e5 100644 --- a/agv_app/app.py +++ b/agv_app/app.py @@ -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) - if m and m.get("back"): - sequence.append({ - "machine_id": m["id"], - "row": r, "col": c, - "side": "back", - "row_dir": "lr", - "row_dir_back": "rl" - }) - 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 m and m.get("front"): - sequence.append({ - "machine_id": m["id"], - "row": r, "col": c, - "side": "front", - "row_dir": "rl", - "row_dir_back": "lr" - }) + 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": pr - 1, "col": c, + "point_row": pr, + "side": "back", + "row_dir": row_dir + }) + 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": pr, "col": c, + "point_row": pr, + "side": "front", + "row_dir": row_dir + }) - return json + return jsonify({"ok": True, "sequence": sequence}) # ========== 点位配置 API（独立于机器）========== @app.route("/api/mission/positions", methods=["GET"]) diff --git a/agv_app/utils/mission_executor.py b/agv_app/utils/mission_executor.py index 79d18b6..e76ab9e 100644 --- a/agv_app/utils/mission_executor.py +++ b/agv_app/utils/mission_executor.py @@ -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: