-

agv_app/utils/mission_executor.py

@@ -253,6 +253,7 @@ class MissionExecutorV3:
                 has_back = pr > 0 and MissionExecutorV3._has_machine(grid, pr - 1, c)
 
                 if not has_front and not has_back:
+                    self._log(f"📍 点位 ({pr},{c}) → 空位")
                     pi += 1
                     continue
 
@@ -522,18 +523,17 @@ class MissionExecutorV3:
     # ==================== 二维码扫描 ====================
 
 
-    def _wait_arm_ready(self, target_angles: list, timeout: float = 10.0, tolerance: float = 3.0) -> bool:
+    def _wait_arm_ready(self, target_angles: list, timeout: float = 15.0, tolerance: float = 2.0) -> bool:
         """等待机械臂稳定到目标角度（±tolerance 度），超时返回 False"""
         if not self.arm_client:
             return True
         deadline = time.time() + timeout
         while time.time() < deadline:
             try:
-                ok, current_rad = self.arm_client.get_angles()
+                ok, current = self.arm_client.get_angles()
-                if ok and current_rad and len(current_rad) >= 6:
+                if ok and current and len(current) >= 6:
-                    # get_angles() 返回弧度，target_angles 是角度，需转换后比较
+                    # get_angles() 返回角度（度），与 target_angles 直接比较
-                    current_deg = [math.degrees(a) for a in current_rad]
+                    if all(abs(current[i] - target_angles[i]) < tolerance for i in range(6)):
-                    if all(abs(current_deg[i] - target_angles[i]) < tolerance for i in range(6)):
                         return True
             except Exception:
                 pass