任务执行

agv_app/app.py

@@ -53,6 +53,7 @@ class GlobalState:
         self.machines_config = []        # 机器配置（每台机器的正面/背面点位+姿态）
         self.qr_config = []               # 二维码配置（独立点位列表）
         self.navigator = None              # Nav2Navigator 实例
+        self.error_msg = ""               # 错误弹窗消息（waiting_error 状态时）
         self.lock = threading.Lock()
 
     def reset(self):
@@ -736,14 +737,16 @@ def api_mission_config_get():
 
 @app.route("/api/mission/config", methods=["POST"])
 def api_mission_config_set():
-    """"设置任务配置（网格尺寸+空位矩阵）"""
+    """"设置任务配置（网格尺寸+空位矩阵+机械臂初始姿态）"""
     data = request.json
     rows = data.get("rows", 2)
     cols = data.get("cols", 3)
     grid = data.get("grid", [])
+    arm_initial_pose = data.get("arm_initial_pose", [0.0] * 6)
     gs.mission_config["rows"] = rows
     gs.mission_config["cols"] = cols
     gs.mission_config["grid"] = grid
+    gs.mission_config["arm_initial_pose"] = arm_initial_pose
     # 清除超出网格边界的 positions（只保留 front/back 且 row<rows, col<cols）
     gs.mission_config["positions"] = [
         p for p in gs.mission_config.get("positions", [])
@@ -1242,6 +1245,9 @@ def api_agv_reset():
 @app.route("/api/mission/start", methods=["POST"])
 def api_mission_start():
     """开始执行任务（V3: M×N Grid 蛇形路径）"""
+    data = request.json or {}
+    single_step = bool(data.get("single_step", False))
+    
     # 清除可能存在的旧实例，确保可以启动
     if hasattr(MissionExecutorV3, "_instance"):
         MissionExecutorV3._instance = None
@@ -1250,7 +1256,7 @@ def api_mission_start():
     if hasattr(MissionExecutorV3, "_instance") and MissionExecutorV3._instance:
         return jsonify({"ok": False, "error": "任务已在运行中"}), 400
 
-    def run():
+    def run(single_step):
         from config import AGV_CONFIG
         config = {
             "device": AGV_CONFIG.get("device", "/dev/agvpro_controller"),
@@ -1275,14 +1281,15 @@ def api_mission_start():
             machines=machines_list,
             qr_configs=gs.qr_config,
             models=models_list,
+            single_step=single_step,
         )
         gs.mission_report = report
         executor.disconnect_all()
         gs.state = State.IDLE if report.get("error") is None else State.PAUSED
 
-    thread = threading.Thread(target=run, daemon=True)
+    thread = threading.Thread(target=run, args=(single_step,), daemon=True)
     thread.start()
-    return jsonify({"ok": True, "message": "任务已启动"})
+    return jsonify({"ok": True, "single_step": single_step})
 
 @app.route("/api/mission/stop", methods=["POST"])
 def api_mission_stop():
@@ -1373,6 +1380,18 @@ def api_mission_state():
     except Exception:
         result["tasks"] = []
 
+    # 错误弹窗状态
+    if hasattr(MissionExecutorV3, "_instance") and MissionExecutorV3._instance:
+        ex = MissionExecutorV3._instance
+        st = ex.get_status()
+        result["error_msg"] = st.get("error", "")
+        result["waiting_step"] = (st.get("status") == "waiting_step")
+        result["waiting_error"] = (st.get("status") == "waiting_error")
+    else:
+        result["error_msg"] = ""
+        result["waiting_step"] = False
+        result["waiting_error"] = False
+
     return jsonify(result)
 
 @app.route("/api/mission/log", methods=["GET"])
@@ -1396,6 +1415,50 @@ def api_mission_manual_qr():
     return jsonify({"ok": False, "error": "没有运行中的任务"}), 400
 
 
+# ========== 错误弹窗 API ==========
+@app.route("/api/mission/error-skip", methods=["POST"])
+def api_mission_error_skip():
+    """用户选择：跳过当前错误"""
+    if hasattr(MissionExecutorV3, "_instance") and MissionExecutorV3._instance:
+        MissionExecutorV3._instance.set_error_choice("skip")
+        return jsonify({"ok": True, "choice": "skip"})
+    return jsonify({"ok": False, "error": "没有运行中的任务"}), 400
+
+@app.route("/api/mission/error-abort", methods=["POST"])
+def api_mission_error_abort():
+    """用户选择：中断任务"""
+    if hasattr(MissionExecutorV3, "_instance") and MissionExecutorV3._instance:
+        MissionExecutorV3._instance.set_error_choice("abort")
+        return jsonify({"ok": True, "choice": "abort"})
+    return jsonify({"ok": False, "error": "没有运行中的任务"}), 400
+
+
+# ========== 单步执行 API ==========
+@app.route("/api/mission/singlestep/confirm", methods=["POST"])
+def api_mission_singlestep_confirm():
+    """单步执行：确认当前步骤正确，继续"""
+    if hasattr(MissionExecutorV3, "_instance") and MissionExecutorV3._instance:
+        MissionExecutorV3._instance.set_step_choice("confirm")
+        return jsonify({"ok": True, "choice": "confirm"})
+    return jsonify({"ok": False, "error": "没有运行中的任务"}), 400
+
+@app.route("/api/mission/singlestep/retry", methods=["POST"])
+def api_mission_singlestep_retry():
+    """单步执行：重试当前步骤"""
+    if hasattr(MissionExecutorV3, "_instance") and MissionExecutorV3._instance:
+        MissionExecutorV3._instance.set_step_choice("retry")
+        return jsonify({"ok": True, "choice": "retry"})
+    return jsonify({"ok": False, "error": "没有运行中的任务"}), 400
+
+@app.route("/api/mission/singlestep/abort", methods=["POST"])
+def api_mission_singlestep_abort():
+    """单步执行：中断任务"""
+    if hasattr(MissionExecutorV3, "_instance") and MissionExecutorV3._instance:
+        MissionExecutorV3._instance.set_step_choice("abort")
+        return jsonify({"ok": True, "choice": "abort"})
+    return jsonify({"ok": False, "error": "没有运行中的任务"}), 400
+
+
 # ========== 二维码配置 API ==========
 @app.route("/api/qr/configs", methods=["GET"])
 def api_qr_configs_get():

agv_app/config.py

@@ -81,6 +81,10 @@ class State:
     RUNNING = "running"
     PAUSED = "paused"
     IDLE = "idle"
+    WAITING_ERROR = "waiting_error"
+    WAITING_STEP = "waiting_step"
+    WAITING_ERROR = "waiting_error"
+    WAITING_STEP = "waiting_step"
 
 class PhotoType:
     FRONT = "front"

agv_app/static/js/running.js

@@ -16,6 +16,11 @@ createApp({
       logs: [],
       showQrModal: false,
       qrValue: '',
+      // 错误弹窗 / 单步执行
+      waitingError: false,
+      errorMsg: '',
+      waitingStep: false,
+      stepLabel: '',
     }
   },
   computed: {
@@ -25,7 +30,9 @@ createApp({
         running: '任务运行中',
         paused: '已暂停',
         completed: '已完成',
-        waiting_qr: '等待输入二维码'
+        waiting_qr: '等待输入二维码',
+        waiting_error: '⚠️ 执行错误',
+        waiting_step: '🦶 等待步骤确认',
       }
       return map[this.missionState] || '未知'
     },
@@ -53,6 +60,22 @@ createApp({
         this.progress = data.progress || 0
         if (data.tasks) this.tasks = data.tasks
 
+        // 错误弹窗
+        if (data.waiting_error) {
+          this.waitingError = true
+          this.errorMsg = data.error_msg || '任务执行出错'
+        } else {
+          this.waitingError = false
+        }
+
+        // 步骤确认弹窗
+        if (data.waiting_step) {
+          this.waitingStep = true
+          this.stepLabel = data.step_label || ''
+        } else {
+          this.waitingStep = false
+        }
+
         // QR 弹窗
         if (this.missionState === 'waiting_qr' && !this.showQrModal) {
           this.showQrModal = true
@@ -68,7 +91,7 @@ createApp({
       } catch (e) {}
     },
     async pollLogs() {
-      if (this.missionState !== 'running' && this.missionState !== 'waiting_qr') return
+      if (this.missionState !== 'running' && this.missionState !== 'waiting_qr' && this.missionState !== 'waiting_error' && this.missionState !== 'waiting_step') return
       try {
         const res = await fetch(API + '/api/mission/log')
         const data = await res.json()
@@ -91,6 +114,40 @@ createApp({
       await fetch(API + '/api/mission/start', { method: 'POST' })
       this.missionState = 'running'
     },
+    async startSingleStep() {
+      if (this.missionState !== 'idle') return
+      this.logs = []
+      this.progress = 0
+      this.report = null
+      this.showQrModal = false
+      await fetch(API + '/api/mission/start', {
+        method: 'POST',
+        headers: { 'Content-Type': 'application/json' },
+        body: JSON.stringify({ single_step: true })
+      })
+      if (this.polling) clearInterval(this.polling)
+      this.poll()
+    },
+    async skipError() {
+      await fetch(API + '/api/mission/error-skip', { method: 'POST' })
+      this.waitingError = false
+    },
+    async abortError() {
+      await fetch(API + '/api/mission/error-abort', { method: 'POST' })
+      this.waitingError = false
+    },
+    async confirmStep() {
+      await fetch(API + '/api/mission/singlestep/confirm', { method: 'POST' })
+      this.waitingStep = false
+    },
+    async retryStep() {
+      await fetch(API + '/api/mission/singlestep/retry', { method: 'POST' })
+      this.waitingStep = false
+    },
+    async abortStep() {
+      await fetch(API + '/api/mission/singlestep/abort', { method: 'POST' })
+      this.waitingStep = false
+    },
     async pauseMission() {
       await fetch(API + '/api/mission/pause', { method: 'POST' })
       this.missionState = 'paused'
@@ -104,6 +161,8 @@ createApp({
       await fetch(API + '/api/mission/stop', { method: 'POST' })
       this.missionState = 'idle'
       this.showQrModal = false
+      this.waitingError = false
+      this.waitingStep = false
     },
     async submitQr() {
       const val = this.qrValue.trim()

agv_app/static/js/setting.js

@@ -59,6 +59,7 @@ createApp({
       qrScanningId: null,
       armCameraUrl: API + '/api/camera/arm_refresh',
       newQrName: '',
+      armInitialPose: [0, 0, 0, 0, 0, 0],
     }
   },
   mounted() {
@@ -489,6 +490,7 @@ createApp({
           this.missionConfig.cols = data.config.cols || 3
           this.missionConfig.grid = data.config.grid || []
           this.missionConfig.positions = data.config.positions || []
+          this.armInitialPose = data.config.arm_initial_pose || [0, 0, 0, 0, 0, 0]
         }
       } catch (e) { console.error('加载任务配置失败', e) }
     },
@@ -520,7 +522,8 @@ createApp({
           body: JSON.stringify({
             rows: this.missionConfig.rows,
             cols: this.missionConfig.cols,
-            grid: this.missionConfig.grid
+            grid: this.missionConfig.grid,
+            arm_initial_pose: this.armInitialPose
           })
         })
         const data = await res.json()
@@ -529,6 +532,33 @@ createApp({
         }
       } catch (e) { alert('保存失败: ' + e.message) }
     },
+    async saveArmInitialPose() {
+      try {
+        const res = await fetch(API + '/api/mission/config', {
+          method: 'POST',
+          headers: { 'Content-Type': 'application/json' },
+          body: JSON.stringify({
+            rows: this.missionConfig.rows,
+            cols: this.missionConfig.cols,
+            grid: this.missionConfig.grid,
+            arm_initial_pose: this.armInitialPose
+          })
+        })
+        const data = await res.json()
+        if (data.ok) alert('✅ 机械臂初始姿态已保存')
+        else alert('❌ 保存失败')
+      } catch (e) { alert('保存失败: ' + e.message) }
+    },
+    async loadArmCurrentAngles() {
+      if (!this.armConnected) { alert('机械臂未连接'); return }
+      try {
+        const res = await fetch(API + '/api/arm/get_angles')
+        const data = await res.json()
+        if (data.ok && data.angles) {
+          this.armInitialPose = [...data.angles]
+        }
+      } catch (e) { alert('读取角度失败: ' + e.message) }
+    },
     async loadAllMachines() {
       try {
         const res = await fetch(API + '/api/mission/machines')

agv_app/templates/running.html

@@ -120,6 +120,31 @@
         </div>
       </div>
 
+      <!-- 错误弹窗 -->
+      <div class="modal-overlay" v-if="waitingError">
+        <div class="modal">
+          <h3>⚠️ 执行错误</h3>
+          <p>[[ errorMsg ]]</p>
+          <div class="modal-actions">
+            <button class="btn btn-warning" @click="skipError">跳过</button>
+            <button class="btn btn-error" @click="abortError">中断</button>
+          </div>
+        </div>
+      </div>
+
+      <!-- 步骤确认弹窗 -->
+      <div class="modal-overlay" v-if="waitingStep">
+        <div class="modal">
+          <h3>🦶 单步执行确认</h3>
+          <p>机器 [[ stepLabel ]] 执行完成，请确认结果是否正确：</p>
+          <div class="modal-actions">
+            <button class="btn btn-success" @click="confirmStep">✅ 正确，继续</button>
+            <button class="btn btn-warning" @click="retryStep">🔄 不正确，重试</button>
+            <button class="btn btn-error" @click="abortStep">⏹️ 中断</button>
+          </div>
+        </div>
+      </div>
+
     </main>
   </div>
 

agv_app/templates/setting.html

@@ -348,6 +348,24 @@
           </div>
         </section>
 
+        <!-- 机械臂初始姿态 -->
+        <section class="card" style="margin-top:16px">
+          <h2>② 🦾 机械臂初始姿态</h2>
+          <p class="hint" style="margin-bottom:12px">每个机器执行前恢复的初始姿态（6个关节角度，单位：度）</p>
+          <div class="form-row" style="flex-wrap:wrap;gap:12px">
+            <div v-for="j in 6" :key="'armInit'+j" class="form-group" style="min-width:100px">
+              <label>J{% raw %}{{ j }}{% endraw %}</label>
+              <input type="number" step="0.5"
+                v-model.number="armInitialPose[j-1]"
+                style="width:100%;padding:8px;border:1px solid #2a3441;border-radius:4px">
+            </div>
+            <div class="form-group" style="align-self:end">
+              <button class="btn btn-primary" @click="saveArmInitialPose">💾 保存初始姿态</button>
+              <button class="btn btn-secondary" @click="loadArmCurrentAngles" :disabled="!armConnected" style="margin-left:6px">📋 读取当前角度</button>
+            </div>
+          </div>
+        </section>
+
         <!-- 下：序列预览 -->
         <section class="card" v-if="sequence && sequence.length > 0" style="margin-top:16px">
           <h2>③ 🐍 蛇形拍摄序列预览</h2>

agv_app/utils/mission_executor.py

@@ -41,6 +41,8 @@ class MissionStatus(str, Enum):
     PAUSED = "paused"
     COMPLETED = "completed"
     WAITING_QR = "waiting_qr"
+    WAITING_ERROR = "waiting_error"
+    WAITING_STEP = "waiting_step"
 
 
 class MissionExecutorV3:
@@ -70,6 +72,22 @@ class MissionExecutorV3:
         self._qr_event = threading.Event()
         self._qr_value: Optional[str] = None
 
+        # 错误弹窗
+        self._error_choice = None  # "skip" or "abort"
+
+        # 单步执行
+        self._single_step_mode = False
+        self._step_choice = None  # "confirm", "retry", "abort"
+        self._error_mode = False  # True when waiting for error resolution
+
+        # 错误弹窗
+        self._error_choice = None  # "skip" or "abort"
+
+        # 单步执行
+        self._single_step_mode = False
+        self._step_choice = None  # "confirm", "retry", "abort"
+        self._error_mode = False  # True when waiting for error resolution
+
         # 设备
         from .arm_client import ArmClient
         self.arm_client = ArmClient(
@@ -104,6 +122,7 @@ class MissionExecutorV3:
         machines: list,
         qr_configs: list,
         models: list,
+        single_step: bool = False,
     ) -> dict:
         """
         执行完整拍摄任务。
@@ -167,25 +186,26 @@ class MissionExecutorV3:
                 "photos_back": 0,
             } for (r, c) in path]
 
-            # 初始化任务列表
+            # 机械臂初始姿态（AGV 移动前恢复）
-            self.report["tasks"] = [{
+            arm_initial_pose = mission_config.get("arm_initial_pose", [0.0] * 6)
-                "row": r, "col": c,
+            has_arm_pose = self.arm_client and any(abs(a) > 0.01 for a in arm_initial_pose)
-                "machine_id": f"m_{r}_{c}",
-                "label": f"{r+1}-{c+1}",
-                "status": "pending",
-                "step": "等待",
-                "qr_value": None,
-                "photos_front": 0,
-                "photos_back": 0,
-            } for (r, c) in path]
 
             # 2. 逐台执行
-            for idx, (r, c) in enumerate(path):
+            machine_idx = 0
+            while machine_idx < len(path):
                 if self._stop.is_set():
                     self._log("⏹️ 任务已停止")
                     break
 
                 self._wait_pause()
+                r, c = path[machine_idx]
+                rl, cl = r + 1, c + 1
+
+                # 恢复机械臂初始姿态
+                if has_arm_pose:
+                    self._log("  🦾 恢复机械臂初始姿态")
+                    self.arm_client.set_angles(arm_initial_pose, speed=500)
+                    time.sleep(2)
 
                 # 更新任务状态 → 正面开始
                 task = self._get_task(r, c)
@@ -196,11 +216,10 @@ class MissionExecutorV3:
                 machine_id = f"m_{r}_{c}"
                 machine = next((m for m in machines if m.get("id") == machine_id), None)
                 if not machine:
-                    self._log(f"⚠️ 机器 {r+1}-{c+1} 不存在，跳过")
+                    self._log(f"⚠️ 机器 {rl}-{cl} 不存在，跳过")
+                    machine_idx += 1
                     continue
 
-                rl, cl = r + 1, c + 1  # 显示用的 1-based
-
                 # --- 正面 ---
                 self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
                 self._step(f"机器 {rl}-{cl} 正面")
@@ -210,6 +229,9 @@ class MissionExecutorV3:
                 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"⚠️ 无正面点位坐标")
 
@@ -233,7 +255,7 @@ class MissionExecutorV3:
                 else:
                     self._log(f"  ⚠️ 未找到机型 {model_name}")
 
-                self._progress(idx, 1)
+                self._progress(machine_idx, 1)
 
                 # --- 背面 ---
                 if task:
@@ -246,6 +268,9 @@ class MissionExecutorV3:
                 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"⚠️ 无背面点位坐标")
 
@@ -257,7 +282,21 @@ class MissionExecutorV3:
                 if task:
                     task["status"] = "completed"
                     task["step"] = "完成"
-                self._progress(idx, 2)
+                self._progress(machine_idx, 2)
+
+                # 单步执行：等待用户确认
+                if single_step and not self._stop.is_set():
+                    choice = self._wait_step_confirm(rl, cl)
+                    if choice == "abort":
+                        break
+                    elif choice == "retry":
+                        if task:
+                            task["status"] = "pending"
+                            task["step"] = "重试开始"
+                        self._progress(machine_idx, 0)
+                        continue  # 不递增 machine_idx，重新执行
+
+                machine_idx += 1
 
             # 3. 回到出发点
             if not self._stop.is_set():
@@ -607,6 +646,82 @@ class MissionExecutorV3:
                 99
             )
 
+    # ==================== 错误弹窗 ====================
+
+    def _wait_error(self, msg: str) -> str:
+        """阻塞等待用户选择：skip（跳过）或 abort（中断）"""
+        self.status = MissionStatus.WAITING_ERROR
+        self.report["status"] = "waiting_error"
+        self.report["step"] = msg
+        self.report["error"] = msg
+        self._log(f"⚠️ 错误处理: {msg}")
+
+        self._error_choice = None
+        self._error_mode = True
+        start = time.time()
+        while self._error_choice is None and not self._stop.is_set():
+            time.sleep(0.2)
+            if time.time() - start > 600:  # 10分钟超时 → 跳过
+                self._error_choice = "skip"
+
+        choice = self._error_choice or "skip"
+        self._error_choice = None
+        self._error_mode = False
+
+        if choice == "abort":
+            self._stop.set()
+            self._log("⏹️ 用户选择中断")
+        else:
+            self._log("⏭️ 用户选择跳过")
+
+        # 恢复状态
+        self.status = MissionStatus.RUNNING if not self._single_step_mode else MissionStatus.WAITING_STEP
+        self.report["status"] = self.status.value
+        self.report["error"] = None
+        return choice
+
+    def set_error_choice(self, choice: str):
+        """外部 API 设置错误处理选择"""
+        self._error_choice = choice
+
+    # ==================== 单步执行 ====================
+
+    def _wait_step_confirm(self, row_label: int, col_label: int) -> str:
+        """单步执行：等待用户确认/重试/中断"""
+        self.status = MissionStatus.WAITING_STEP
+        self.report["status"] = "waiting_step"
+        self.report["step"] = f"机器 {row_label}-{col_label} 完成，等待确认"
+        self.report["current_step"] = {
+            "row": row_label - 1, "col": col_label - 1,
+            "label": f"{row_label}-{col_label}"
+        }
+        self._log(f"⏸️ 单步执行: 机器 {row_label}-{col_label} 完成，等待确认...")
+
+        self._step_choice = None
+        start = time.time()
+        while self._step_choice is None and not self._stop.is_set():
+            time.sleep(0.2)
+            if time.time() - start > 600:  # 10分钟超时 → 确认
+                self._step_choice = "confirm"
+
+        choice = self._step_choice or "confirm"
+        self._step_choice = None
+        self.report.pop("current_step", None)
+
+        if choice == "abort":
+            self._stop.set()
+            self._log("⏹️ 用户选择中断")
+        elif choice == "retry":
+            self._log(f"🔄 用户选择重试机器 {row_label}-{col_label}")
+        else:
+            self._log(f"✅ 用户确认机器 {row_label}-{col_label}")
+
+        return choice
+
+    def set_step_choice(self, choice: str):
+        """外部 API 设置单步执行选择"""
+        self._step_choice = choice
+
     # ==================== Nav2 导航 ====================
     # (保留原实现)