修复位置和返回问题

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)
+    rows = int(gs.mission_config.get("rows", 2))
-    cols = gs.mission_config.get("cols", 3)
+    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):
+    for pr in range(rows + 1):
-        # 检查该行是否有机器
+        cols_iter = range(cols) if pr % 2 == 0 else range(cols - 1, -1, -1)
-        has_any = any(grid[r][c] for c in range(cols)) if r < len(grid) else False
+        row_dir = "lr" if pr % 2 == 0 else "rl"
-        if not has_any:
+        for c in cols_iter:
-            continue
+            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
+                    })
 
-        if r % 2 == 0:  # 偶数行：正面从左到右，背面从右到左
+    return jsonify({"ok": True, "sequence": sequence})
-            # 正面：从左到右
-            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"
-                        })
-
-    return json
 
 # ========== 点位配置 API（独立于机器）==========
 @app.route("/api/mission/positions", methods=["GET"])

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
+                if not self._return_to_origin():
-                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:
                     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: