Update mission flow and inspection log scrolling

agv_app/app.py

@@ -46,6 +46,15 @@ logging.basicConfig(
 )
 logger = logging.getLogger("agv_app")
 
+ORIGIN_X = 0.0
+ORIGIN_Y = 0.0
+ORIGIN_YAW = 0.0
+ORIGIN_MATCH_TOLERANCE = 1e-6
+ARM_RETURN_SPEED = 500
+ARM_INITIAL_POSE_TIMEOUT = 15.0
+ARM_INITIAL_POSE_TOLERANCE = 2.0
+ARM_INITIAL_POSE_UNSET_TOLERANCE = 0.01
+
 app = Flask(__name__, template_folder="templates", static_folder="static", static_url_path="/static")
 app.config["SECRET_KEY"] = SERVER_CONFIG["secret_key"]
 CORS(app)
@@ -88,6 +97,60 @@ gs = GlobalState()
 
 
 # ========== 辅助函数 ==========
+def _is_origin_goal(x: float, y: float, yaw: float = None) -> bool:
+    yaw_is_origin = yaw is None or abs(yaw - ORIGIN_YAW) <= ORIGIN_MATCH_TOLERANCE
+    return (
+        abs(x - ORIGIN_X) <= ORIGIN_MATCH_TOLERANCE
+        and abs(y - ORIGIN_Y) <= ORIGIN_MATCH_TOLERANCE
+        and yaw_is_origin
+    )
+
+
+def _normalize_arm_pose(raw_pose):
+    if not isinstance(raw_pose, list) or len(raw_pose) != 6:
+        return None
+    try:
+        return [float(angle) for angle in raw_pose]
+    except (TypeError, ValueError):
+        return None
+
+
+def _wait_arm_initial_pose(target_angles, timeout=ARM_INITIAL_POSE_TIMEOUT, tolerance=ARM_INITIAL_POSE_TOLERANCE):
+    deadline = time.time() + timeout
+    while time.time() < deadline:
+        try:
+            ok, current_angles = gs.arm_client.get_angles()
+            if ok and current_angles and len(current_angles) >= 6:
+                current_angles = [float(angle) for angle in current_angles[:6]]
+                if all(abs(current_angles[index] - target_angles[index]) <= tolerance for index in range(6)):
+                    return True
+        except Exception as e:
+            logger.warning(f"等待机械臂初始姿态失败: {e}")
+        time.sleep(0.5)
+    return False
+
+
+def _restore_arm_initial_pose_for_origin():
+    arm_initial_pose = _normalize_arm_pose(gs.mission_config.get("arm_initial_pose"))
+    if not arm_initial_pose:
+        return {"ok": True, "skipped": True, "message": "未配置有效机械臂初始姿态，跳过复原"}
+    if not any(abs(angle) > ARM_INITIAL_POSE_UNSET_TOLERANCE for angle in arm_initial_pose):
+        return {"ok": True, "skipped": True, "message": "未配置机械臂初始姿态，跳过复原"}
+    if not gs.arm_client:
+        return {"ok": False, "error": "机械臂未连接，无法先复原再回原点"}
+
+    try:
+        ok = gs.arm_client.set_angles(arm_initial_pose, ARM_RETURN_SPEED)
+        if not ok:
+            return {"ok": False, "error": "机械臂初始姿态指令发送失败，已取消回原点"}
+        if not _wait_arm_initial_pose(arm_initial_pose):
+            return {"ok": False, "error": "机械臂未在规定时间内复原，已取消回原点"}
+        return {"ok": True, "skipped": False, "message": "机械臂已复原到初始姿态"}
+    except Exception as e:
+        logger.error(f"回原点前复原机械臂失败: {e}")
+        return {"ok": False, "error": f"回原点前复原机械臂失败: {e}"}
+
+
 def get_data_path(name):
     return os.path.join(DATA_DIR, name)
 
@@ -464,24 +527,38 @@ def api_navigate_to():
     if not gs.map_config or "map_yaml" not in gs.map_config:
         return jsonify({"ok": False, "error": "地图未加载，请先在设置中加载地图"}), 400
 
-    data = request.json
+    data = request.json or {}
     goal_x = data.get("x")
     goal_y = data.get("y")
     goal_yaw = data.get("yaw")  # 姿态参数，可选
     if goal_x is None or goal_y is None:
         return jsonify({"ok": False, "error": "缺少目标坐标 x, y"}), 400
+    try:
+        goal_x = float(goal_x)
+        goal_y = float(goal_y)
+        yaw_arg = float(goal_yaw) if goal_yaw is not None else None
+    except (TypeError, ValueError):
+        return jsonify({"ok": False, "error": "目标坐标格式错误"}), 400
 
     if not gs.agv_controller or not gs.agv_controller.is_connected():
         return jsonify({"ok": False, "error": "AGV 未连接，请先连接 AGV"}), 400
 
     try:
+        restore_message = ""
+        restore_arm = bool(data.get("restore_arm", True))
+        if restore_arm and _is_origin_goal(goal_x, goal_y, yaw_arg):
+            restore_result = _restore_arm_initial_pose_for_origin()
+            if not restore_result["ok"]:
+                return jsonify({"ok": False, "error": restore_result["error"]}), 400
+            restore_message = restore_result.get("message", "")
+
         if gs.navigator is None:
             gs.navigator = Nav2Navigator()
         # navigate_to_pose(x, y, yaw=None, timeout_sec=120, blocking=False)
-        yaw_arg = float(goal_yaw) if goal_yaw is not None else None
-        ok = gs.navigator.navigate_to_pose(float(goal_x), float(goal_y), yaw_arg, blocking=False)
+        ok = gs.navigator.navigate_to_pose(goal_x, goal_y, yaw_arg, blocking=False)
         if ok:
-            return jsonify({"ok": True, "message": "导航已启动"})
+            message = f"{restore_message}，导航已启动" if restore_message else "导航已启动"
+            return jsonify({"ok": True, "message": message})
         else:
             return jsonify({"ok": False, "error": "导航启动失败，可能是Nav2未运行或AGV未连接"}), 400
     except Exception as e: