diff --git a/agv_app/config.py b/agv_app/config.py index 0e9966f..ada87a7 100644 --- a/agv_app/config.py +++ b/agv_app/config.py @@ -29,7 +29,7 @@ ARM_CONFIG = { # ========== 地图 ========== MAP_CONFIG = { - "map_dir": "/home/elephant/agv_pro_ros2/install/agv_pro_navigation2/share/agv_pro_navigation2/map/", + "map_dir": "/home/elephant/agv_pro_ros2/src/agv_pro_navigation2/map/", "map_file": "map.yaml", } @@ -85,10 +85,6 @@ 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" diff --git a/agv_app/data/machines_config.json b/agv_app/data/machines_config.json index 39e6ce9..abd1625 100644 --- a/agv_app/data/machines_config.json +++ b/agv_app/data/machines_config.json @@ -1,46 +1,4 @@ [ - { - "id": "m_0_2", - "row": 0, - "col": 2, - "front": { - "coords": [ - 1.2421705407118802, - 0.0025490140048510445, - 0.00150923641 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, - { - "id": "m_1_2", - "row": 1, - "col": 2, - "front": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, { "id": "m_2_0", "row": 2, @@ -209,111 +167,6 @@ "poses": [] } }, - { - "id": "m_0_1", - "row": 0, - "col": 1, - "front": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, - { - "id": "m_0_0", - "row": 0, - "col": 0, - "front": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, - { - "id": "m_1_1", - "row": 1, - "col": 1, - "front": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, - { - "id": "m_1_0", - "row": 1, - "col": 0, - "front": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, - { - "id": "m_0_3", - "row": 0, - "col": 3, - "front": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, { "id": "m_0_4", "row": 0, @@ -356,27 +209,6 @@ "poses": [] } }, - { - "id": "m_1_3", - "row": 1, - "col": 3, - "front": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, { "id": "m_1_5", "row": 1, @@ -398,27 +230,6 @@ "poses": [] } }, - { - "id": "m_1_4", - "row": 1, - "col": 4, - "front": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - "back": { - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - } - }, { "id": "m_2_3", "row": 2, @@ -502,5 +313,167 @@ ], "poses": [] } + }, + { + "id": "m_0_0", + "row": 0, + "col": 0, + "front": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "back": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + } + }, + { + "id": "m_1_0", + "row": 1, + "col": 0, + "front": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "back": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + } + }, + { + "id": "m_0_1", + "row": 0, + "col": 1, + "front": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "back": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "qr": { + "coords": [ + 0, + 0, + 0 + ], + "qr_value": "", + "model_id": "" + } + }, + { + "id": "m_0_2", + "row": 0, + "col": 2, + "front": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "back": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "qr": { + "coords": [ + 0, + 0, + 0 + ], + "qr_value": "", + "model_id": "" + } + }, + { + "id": "m_0_3", + "row": 0, + "col": 3, + "front": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "back": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "qr": { + "coords": [ + 0, + 0, + 0 + ], + "qr_value": "", + "model_id": "" + } + }, + { + "id": "m_1_1", + "row": 1, + "col": 1, + "front": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "back": { + "coords": [ + 0, + 0, + 0 + ], + "poses": [] + }, + "qr": { + "coords": [ + 0, + 0, + 0 + ], + "qr_value": "", + "model_id": "" + } } ] \ No newline at end of file diff --git a/agv_app/data/map_config.json b/agv_app/data/map_config.json index 7ccc11d..8383bfb 100644 --- a/agv_app/data/map_config.json +++ b/agv_app/data/map_config.json @@ -1,5 +1,5 @@ { - "map_dir": "/home/elephant/agv_pro_ros2/src/agv_pro_navigation2/map/", + "map_dir": "/home/elephant/agv_pro_ros2/install/agv_pro_navigation2/share/agv_pro_navigation2/map/", "map_file": "map.yaml", - "map_yaml": "/home/elephant/agv_pro_ros2/src/agv_pro_navigation2/map/map.yaml" + "map_yaml": "/home/elephant/agv_pro_ros2/install/agv_pro_navigation2/share/agv_pro_navigation2/map/map.yaml" } \ No newline at end of file diff --git a/agv_app/data/mission_config.json b/agv_app/data/mission_config.json index fe42feb..79f78bd 100644 --- a/agv_app/data/mission_config.json +++ b/agv_app/data/mission_config.json @@ -1,6 +1,6 @@ { - "rows": 4, - "cols": 6, + "rows": 2, + "cols": 5, "grid": [], "positions": [ { @@ -8,15 +8,169 @@ "col": 0, "side": "front", "coords": [ - 0.616485726055098, - -0.002587517923224651, - -0.003483980050000001 + 0.7790541397954953, + -1.4475345726592936, + 0.04652525663733687 ], "poses": [] }, { - "row": 3, + "row": 0, "col": 1, + "side": "front", + "coords": [ + 1.7721798197110892, + -1.4172647050697822, + 0.026346988400783075 + ], + "poses": [] + }, + { + "row": 1, + "col": 1, + "side": "back", + "coords": [ + 2.0156283932242465, + -3.6286459327051652, + -3.1344357375846337 + ], + "poses": [] + }, + { + "row": 1, + "col": 0, + "side": "back", + "coords": [ + 0.6542779107672178, + -3.610037554246379, + -3.139991515496645 + ], + "poses": [] + }, + { + "row": 0, + "col": 2, + "side": "front", + "coords": [ + 3.032329928935363, + -1.4045725439516354, + 0.019396580473191316 + ], + "poses": [] + }, + { + "row": 0, + "col": 3, + "side": "front", + "coords": [ + 4.355614139761155, + -1.4037408856787776, + 0.028879849807957422 + ], + "poses": [] + }, + { + "row": 0, + "col": 4, + "side": "front", + "coords": [ + 5.591022741827066, + -1.4041991720435099, + 0.007950673643717402 + ], + "poses": [] + }, + { + "row": 1, + "col": 4, + "side": "back", + "coords": [ + 5.855580527635482, + -3.653374052468203, + -3.1205683154447357 + ], + "poses": [] + }, + { + "row": 1, + "col": 4, + "side": "front", + "coords": [ + 5.855580527635482, + -3.653374052468203, + -3.1205683154447357 + ], + "poses": [] + }, + { + "row": 1, + "col": 3, + "side": "back", + "coords": [ + 4.639109284799733, + -3.6545643029563504, + -3.103938599592417 + ], + "poses": [] + }, + { + "row": 1, + "col": 3, + "side": "front", + "coords": [ + 4.639109284799733, + -3.6545643029563504, + -3.103938599592417 + ], + "poses": [] + }, + { + "row": 1, + "col": 2, + "side": "back", + "coords": [ + 3.352049221669485, + -3.6285597877078626, + 3.1386114980262145 + ], + "poses": [] + }, + { + "row": 1, + "col": 2, + "side": "front", + "coords": [ + 3.352049221669485, + -3.6285597877078626, + 3.1386114980262145 + ], + "poses": [] + }, + { + "row": 1, + "col": 1, + "side": "front", + "coords": [ + 2.0156283932242465, + -3.6286459327051652, + -3.1344357375846337 + ], + "poses": [] + }, + { + "row": 1, + "col": 0, + "side": "front", + "coords": [ + 0.6542779107672178, + -3.610037554246379, + -3.139991515496645 + ], + "poses": [] + }, + { + "row": 2, + "col": 0, "side": "back", "coords": [ 0, @@ -25,98 +179,10 @@ ], "poses": [] }, - { - "row": 1, - "col": 1, - "side": "front", - "coords": [ - -0.27906358987415997, - 0.00411087876725537, - -0.00749475593 - ], - "poses": [] - }, - { - "row": 0, - "col": 1, - "side": "front", - "coords": [ - 0.616485726055098, - -0.002587517923224651, - -0.003483980050000001 - ], - "poses": [] - }, - { - "row": 0, - "col": 2, - "side": "front", - "coords": [ - -0.27906358987415997, - 0.00411087876725537, - -0.00749475593 - ], - "poses": [] - }, { "row": 2, "col": 1, - "side": "shoot", - "coords": [ - -1.898244121263206, - -0.014324627152337432, - 0.004533442980000002 - ], - "poses": [] - }, - { - "row": 2, - "col": 0, - "side": "shoot", - "coords": [ - -0.9528404539697249, - -0.01004755255507813, - 0.005515614170000002 - ], - "poses": [] - }, - { - "row": 0, - "col": 1, - "side": "shoot", - "coords": [ - -0.9528404539697249, - -0.01004755255507813, - 0.005515614170000002 - ], - "poses": [] - }, - { - "row": 1, - "col": 1, - "side": "shoot", - "coords": [ - -0.9528404539697249, - -0.01004755255507813, - 0.005515614170000002 - ], - "poses": [] - }, - { - "row": 0, - "col": 0, - "side": "shoot", - "coords": [ - 0, - 0, - 0 - ], - "poses": [] - }, - { - "row": 1, - "col": 0, - "side": "shoot", + "side": "back", "coords": [ 0, 0, @@ -124,5 +190,13 @@ ], "poses": [] } + ], + "arm_initial_pose": [ + -90.333323, + -90.079952, + 0.160037, + -90.571318, + 0.093654, + 22.232898 ] } \ No newline at end of file diff --git a/agv_app/data/models_config.json b/agv_app/data/models_config.json index 8a6b3c1..fb15d33 100644 --- a/agv_app/data/models_config.json +++ b/agv_app/data/models_config.json @@ -9,12 +9,12 @@ "name": "front_1", "photo_type": "front", "arm_angles": [ - 0, - 0, - 0, - 0, - 0, - 0 + 46.333359, + -90.759907, + 7.5e-05, + -89.156729, + -15.823339, + 20.403326 ], "speed": 500, "description": "" @@ -24,12 +24,12 @@ "name": "front_2", "photo_type": "front", "arm_angles": [ - 0, - 0, - 0, - 0, - 0, - 0 + 46.333325, + -88.800201, + 0.000164, + -89.1564, + -15.823202, + 20.403374 ], "speed": 500, "description": "" @@ -67,5 +67,13 @@ ], "description": "", "notes": "" + }, + { + "id": "m_1779286436", + "name": "m2", + "serial_prefix": "", + "poses": [], + "description": "", + "notes": "" } ] \ No newline at end of file diff --git a/agv_app/mission_executor.py b/agv_app/mission_executor.py index efc3f0f..d0459e2 100644 --- a/agv_app/mission_executor.py +++ b/agv_app/mission_executor.py @@ -23,6 +23,8 @@ import requests import cv2 import numpy as np +from utils.nav2_navigator import Nav2Navigator, Nav2Status + logger = logging.getLogger(__name__) ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash" @@ -42,6 +44,8 @@ class MissionStatus(str, Enum): PAUSED = "paused" COMPLETED = "completed" WAITING_QR = "waiting_qr" + WAITING_ERROR = "waiting_error" + WAITING_STEP = "waiting_step" class MissionExecutorV3: @@ -71,6 +75,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( @@ -84,6 +104,9 @@ class MissionExecutorV3: baudrate=config.get("baudrate", 1000000) ) + # Nav2 导航器（直接使用 rclpy BasicNavigator API，比 subprocess 更可靠） + self._nav = Nav2Navigator() + # ==================== 连接 ==================== def connect_all(self) -> Dict[str, bool]: @@ -105,10 +128,23 @@ class MissionExecutorV3: machines: list, qr_configs: list, models: list, + single_step: bool = False, + options: dict = None, ) -> dict: """ 执行完整拍摄任务。 + Args: + options: 任务步骤控制开关 + - arm_init: 是否执行机械臂位置初始化 + - agv_move: 是否执行AGV移动 + - qr_scan: 是否执行二维码识别 + - front_photo: 是否执行正面拍照 + - back_photo: 是否执行背面拍照 + """ + """ + 执行完整拍摄任务。 + Args: mission_config: {rows, cols, grid, positions} machines: [{id, row, col, front: {coords}, back: {coords}}] @@ -131,8 +167,23 @@ class MissionExecutorV3: grid = mission_config.get("grid", []) positions = mission_config.get("positions", []) + # 如果 grid 为空，从 machines 重建 + if not grid or all(not any(rw) if isinstance(rw, list) else True for rw in grid): + try: + cfg_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "data") + with open(os.path.join(cfg_dir, "machines_config.json")) as jf: + machines = json.load(jf) + 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 + except Exception: + grid = [[False] * cols for _ in range(rows)] + # 1. 生成蛇形路径 - path = self._build_snake_path(rows, cols, grid) + path = MissionExecutorV3._build_snake_path(rows, cols, grid) if not path: self._log("❌ 网格中没有机器，任务终止") self.report["error"] = "No machines in grid" @@ -153,25 +204,43 @@ class MissionExecutorV3: "photos_back": 0, } for (r, c) in path] - # 初始化任务列表 - self.report["tasks"] = [{ - "row": r, "col": c, - "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] + # 任务步骤控制开关 + if options is None: + options = {} + opt_arm_init = options.get("arm_init", True) + opt_agv_move = options.get("agv_move", True) + # 机械臂初始化并入 AGV 移动：只要开移动，就先初始化机械臂 + if opt_agv_move: + opt_arm_init = True + opt_qr_scan = options.get("qr_scan", True) + opt_front_photo = options.get("front_photo", True) + opt_back_photo = options.get("back_photo", True) + self._log(f"⚙️ 任务步骤: agv_move={opt_agv_move}, " + f"qr_scan={opt_qr_scan}, front={opt_front_photo}, back={opt_back_photo}") + + # 机械臂初始姿态（AGV 移动前恢复） + arm_initial_pose = mission_config.get("arm_initial_pose", [0.0] * 6) + has_arm_pose = self.arm_client and any(abs(a) > 0.01 for a in arm_initial_pose) # 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 opt_arm_init and has_arm_pose: + self._log(" 🦾 恢复机械臂初始姿态") + try: + self.arm_client.set_angles(arm_initial_pose, speed=500) + time.sleep(2) + except Exception as e: + self._log(f" ⚠️ 机械臂初始化失败: {e}") # 更新任务状态 → 正面开始 task = self._get_task(r, c) @@ -182,27 +251,36 @@ 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} 正面") # 导航到正面点位 - front_pt = self._find_point(positions, r, c, "front") - if front_pt and self._has_coords(front_pt): - if not self._navigate(front_pt, "正面"): - self._log(f"⚠️ 导航失败，尝试继续") + if opt_agv_move: + front_pt = self._find_point(positions, r, c, "front") + 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"⚠️ 无正面点位坐标") else: - self._log(f"⚠️ 无正面点位坐标") + self._log(" ⏭️ 跳过AGV移动（正面）") # 扫描二维码 - qr_value = self._scan_qr_with_poses(qr_configs) - if self._stop.is_set(): - break + qr_value = None + if opt_qr_scan: + qr_value = self._scan_qr_with_poses(qr_configs) + if self._stop.is_set(): + break + else: + self._log(" ⏭️ 跳过二维码识别") # 查机型 + 更新任务步骤 model_name = self._lookup_model(qr_value) @@ -213,13 +291,16 @@ class MissionExecutorV3: task["step"] = "正面拍照" # 正面拍照 - model = self._find_model(models, model_name) - if model: - self._shoot(model, "front", rl, cl, qr_value or "unknown") + if opt_front_photo: + model = self._find_model(models, model_name) + if model: + self._shoot(model, "front", rl, cl, qr_value or "unknown") + else: + self._log(f" ⚠️ 未找到机型 {model_name}") else: - self._log(f" ⚠️ 未找到机型 {model_name}") + self._log(" ⏭️ 跳过正面拍照") - self._progress(idx, 1) + self._progress(machine_idx, 1) # --- 背面 --- if task: @@ -228,28 +309,53 @@ class MissionExecutorV3: self._step(f"机器 {rl}-{cl} 背面") # 导航到背面点位 - back_pt = self._find_point(positions, r + 1, c, "back") - if back_pt and self._has_coords(back_pt): - if not self._navigate(back_pt, "背面"): - self._log(f"⚠️ 导航失败，尝试继续") + if opt_agv_move: + back_pt = self._find_point(positions, r + 1, c, "back") + 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"⚠️ 无背面点位坐标") else: - self._log(f"⚠️ 无背面点位坐标") + self._log(" ⏭️ 跳过AGV移动（背面）") # 背面拍照 - if model: - self._shoot(model, "back", rl, cl, qr_value or "unknown") + if opt_back_photo: + if model: + self._shoot(model, "back", rl, cl, qr_value or "unknown") + else: + self._log(" ⏭️ 跳过背面拍照") # 标记任务完成 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(): + if not self._stop.is_set() and opt_agv_move: self._step("返回出发点") self._log("→ 返回 (0, 0)") self._nav2_go_to_point(0, 0, 0, timeout_sec=60) + elif not self._stop.is_set(): + self._log("⏭️ 跳过返回出发点") elapsed = time.time() - start_time return self._finish(elapsed) @@ -275,17 +381,18 @@ class MissionExecutorV3: # ==================== 蛇形路径 ==================== - def _build_snake_path(self, rows: int, cols: int, grid: list) -> list: + @staticmethod + def _build_snake_path(rows: int, cols: int, grid: list) -> list: """奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左""" path = [] for r in range(rows): if r % 2 == 0: for c in range(cols): - if self._has_machine(grid, r, c): + if MissionExecutorV3._has_machine(grid, r, c): path.append((r, c)) else: for c in range(cols - 1, -1, -1): - if self._has_machine(grid, r, c): + if MissionExecutorV3._has_machine(grid, r, c): path.append((r, c)) return path @@ -298,6 +405,52 @@ class MissionExecutorV3: return c < len(row) and bool(row[c]) return False + @staticmethod + def _build_grid_from_machines(rows: int, cols: int, machines: list) -> list: + """从机器列表重建 grid 矩阵""" + if not machines: + return [[False] * cols for _ in range(rows)] + max_r = max(int(m.get("row", 0)) for m in machines) + 1 + max_c = max(int(m.get("col", 0)) for m in machines) + 1 + gr = max(rows, max_r) + gc = max(cols, max_c) + grid = [[False] * gc for _ in range(gr)] + for m in machines: + r = int(m.get("row", 0)) + c = int(m.get("col", 0)) + if 0 <= r < gr and 0 <= c < gc: + grid[r][c] = True + return grid + + @staticmethod + def pre_generate_tasks(mission_config: dict) -> list: + """从网格配置预生成任务列表（用于 UI 展示，无需启动执行器）""" + rows = int(mission_config.get("rows", 1)) + cols = int(mission_config.get("cols", 1)) + grid = mission_config.get("grid", []) + + # 如果 grid 为空但从 machines 重建 + if not grid and machines: + grid = MissionExecutorV3._build_grid_from_machines(rows, cols, machines) + if grid: + rows = len(grid) + cols = len(grid[0]) if grid else cols + + path = MissionExecutorV3._build_snake_path(rows, cols, grid) + tasks = [] + for (r, c) in path: + tasks.append({ + "row": r, "col": c, + "machine_id": f"m_{r}_{c}", + "label": f"{r+1}-{c+1}", + "status": "pending", + "step": "等待", + "qr_value": None, + "photos_front": 0, + "photos_back": 0, + }) + return tasks + # ==================== 点位查找 ==================== @staticmethod @@ -512,6 +665,7 @@ class MissionExecutorV3: "step": self.report["step"], "progress": self.report["progress"], "total": self.report["total"], + "tasks": self.report.get("tasks", []), } def get_logs(self) -> dict: @@ -545,6 +699,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 导航 ==================== # (保留原实现) @@ -559,72 +789,15 @@ class MissionExecutorV3: def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0, timeout_sec: float = 120.0) -> bool: - if not self._nav2_check_available(): - logger.error("Nav2 action server 不可用") - return False - - qz = math.sin(yaw / 2.0) - qw = math.cos(yaw / 2.0) - - pose_yaml = ( - f"pose:\n" - f" header:\n" - f" stamp:\n" - f" sec: 0\n" - f" nanosec: 0\n" - f" frame_id: 'map'\n" - f" pose:\n" - f" position:\n" - f" x: {x}\n" - f" y: {y}\n" - f" z: 0.0\n" - f" orientation:\n" - f" x: 0.0\n" - f" y: 0.0\n" - f" z: {qz}\n" - f" w: {qw}" - ) - - cmd = ( - f"ros2 action send_goal /navigate_to_pose " - f"nav2_msgs/action/NavigateToPose " - f"'{pose_yaml}' --feedback" - ) - full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'" - + """使用 Nav2Navigator 直接发送导航目标（blocking 模式，等待完成）""" try: - process = subprocess.Popen( - full_cmd, shell=True, - stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True - ) - succeeded = False - elapsed = 0.0 - while elapsed < timeout_sec: - import select - reads = [process.stdout] - ready, _, _ = select.select(reads, [], [], 1.0) - elapsed += 1.0 - if process.stdout in ready: - line = process.stdout.readline() - if not line: - break - ls = line.strip() - if "succeeded" in ls.lower(): - succeeded = True - break - elif "failed" in ls.lower() or "aborted" in ls.lower(): - break - elif "canceled" in ls.lower() or "cancelled" in ls.lower(): - break - if process.poll() is not None: - break - if process.poll() is None: - process.terminate() - try: - process.wait(timeout=3) - except subprocess.TimeoutExpired: - process.kill() - return succeeded + 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) + if ok: + logger.info(f"✅ 导航成功到达 ({x:.3f}, {y:.3f})") + else: + logger.warning(f"⚠️ 导航失败或超时 ({x:.3f}, {y:.3f})") + return ok except Exception as e: logger.error(f"Nav2 异常: {e}") return False diff --git a/agv_app/running.html b/agv_app/running.html index 5536159..a77a9d9 100644 --- a/agv_app/running.html +++ b/agv_app/running.html @@ -4,7 +4,7 @@ 运行监控 - AGV 拍摄系统 - +
@@ -48,6 +48,46 @@
+ +
+

🎛️ 任务步骤控制

+

关闭的步骤将在本次任务中跳过

+
+
+ + 🚗 AGV移动 + 含机械臂初始化，按之字形路线移动到各点位 +
+
+ + 📷 识别二维码 + 调整机械臂姿态扫描二维码 +
+
+ + 📸 拍正面照 + 按机型正面姿态拍照 +
+
+ + 📸 拍背面照 + 按机型背面姿态拍照 +
+
+
+

📋 任务清单 ([[ tasks.length ]] 台机器)

@@ -85,8 +125,15 @@

📷 摄像头预览

-
- +
+
+
🎥 AGV 摄像头
+ +
+
+
🦾 机械臂摄像头
+ +
@@ -113,10 +160,35 @@ + +
+
+

⚠️ 执行错误

+

[[ errorMsg ]]

+
+ + +
+
+
+ + +
+
+

🦶 单步执行确认

+

机器 [[ stepLabel ]] 执行完成，请确认结果是否正确：

+
+ + + +
+
+
+ - + \ No newline at end of file diff --git a/agv_app/running.js b/agv_app/running.js index aa01478..72af4d5 100644 --- a/agv_app/running.js +++ b/agv_app/running.js @@ -10,11 +10,22 @@ createApp({ progress: 0, tasks: [], report: null, - previewUrl: API + '/api/camera/preview', + agvPreviewUrl: API + '/api/camera/preview', + armPreviewUrl: API + '/api/camera/arm_refresh', polling: null, logs: [], showQrModal: false, qrValue: '', + // 错误弹窗 / 单步执行 + waitingError: false, + errorMsg: '', + waitingStep: false, + stepLabel: '', + // 任务步骤控制开关（机械臂初始化并入AGV移动） + agvMoveEnabled: true, + qrScanEnabled: true, + frontPhotoEnabled: true, + backPhotoEnabled: true, } }, computed: { @@ -24,7 +35,9 @@ createApp({ running: '任务运行中', paused: '已暂停', completed: '已完成', - waiting_qr: '等待输入二维码' + waiting_qr: '等待输入二维码', + waiting_error: '⚠️ 执行错误', + waiting_step: '🦶 等待步骤确认', } return map[this.missionState] || '未知' }, @@ -52,6 +65,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 @@ -67,7 +96,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() @@ -87,9 +116,52 @@ createApp({ this.progress = 0 this.report = null this.showQrModal = false - await fetch(API + '/api/mission/start', { method: 'POST' }) + await fetch(API + '/api/mission/start', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ + agv_move: this.agvMoveEnabled, + qr_scan: this.qrScanEnabled, + front_photo: this.frontPhotoEnabled, + back_photo: this.backPhotoEnabled, + }) + }) 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' @@ -103,6 +175,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() @@ -123,7 +197,10 @@ createApp({ body: JSON.stringify({ qr: 'SKIP' }) }) }, - onPreviewError(e) { + onAgvPreviewError(e) { + e.target.style.display = 'none' + }, + onArmPreviewError(e) { e.target.style.display = 'none' } } diff --git a/agv_app/setting.html b/agv_app/setting.html index 41db5f6..00126d2 100644 --- a/agv_app/setting.html +++ b/agv_app/setting.html @@ -4,7 +4,7 @@ 设置 - AGV 拍摄系统 - +
@@ -12,7 +12,7 @@ @@ -135,7 +135,7 @@

🟢 正面姿态

-
+
{% raw %}{{ pose.name || '正面姿态' }}{% endraw %} @@ -182,7 +182,7 @@

🔴 背面姿态

-
+
{% raw %}{{ pose.name || '背面姿态' }}{% endraw %} @@ -268,6 +268,7 @@ +
@@ -348,116 +349,22 @@
- -
-

② 点位配置 — 第{% raw %}{{ selectedMachine.row+1 }}{% endraw %}行 第{% raw %}{{ selectedMachine.col+1 }}{% endraw %}列

- - -
-

📷 正面点位

-
-
- - -
-
- - -
-
- - -
-
- -
+ +
+

② 🦾 机械臂初始姿态

+

每个机器执行前恢复的初始姿态（6个关节角度，单位：度）

+
+
+ +
- -
-

正面姿态 ({% raw %}{{ selectedMachine.front.poses.length }}{% endraw %} 个)

-
- {% raw %}{{ pose.name }}{% endraw %} - 角度: {% raw %}{{ formatAngles(pose.arm_angles) }}{% endraw %} - -
+
+ + +
-
- - -
-
- - -
-

📷 背面点位

-
-
- - -
-
- - -
-
- - -
-
- -
-
- -
-

背面姿态 ({% raw %}{{ selectedMachine.back.poses.length }}{% endraw %} 个)

-
- {% raw %}{{ pose.name }}{% endraw %} - 角度: {% raw %}{{ formatAngles(pose.arm_angles) }}{% endraw %} - -
-
-
- - -
-
- - -
-

🔍 二维码位置

-
-
- - -
-
- - -
-
- - -
-
- -
-
- -
- 📱 二维码值: {% raw %}{{ safeQr('qr_value') }}{% endraw %} - 🏷️ 匹配机型: {% raw %}{{ safeQrModelName() }}{% endraw %} - ⚠️ 未匹配到机型 -
-
- -
-
- -
- -
@@ -676,7 +583,7 @@
- - + + diff --git a/agv_app/setting.js b/agv_app/setting.js index d8e8303..ac59bb0 100644 --- a/agv_app/setting.js +++ b/agv_app/setting.js @@ -26,6 +26,9 @@ createApp({ newPointName: '', newPointMode: 'front', newPointSequence: ['front', 'back'], + // 点位编辑器弹窗 + editingPoint: null, + pointEditor: { x: 0, y: 0, yaw: 0 }, // 机型（姿态组） models: [], selectedModelId: null, @@ -56,6 +59,7 @@ createApp({ qrScanningId: null, armCameraUrl: API + '/api/camera/arm_refresh', newQrName: '', + armInitialPose: [0, 0, 0, 0, 0, 0], } }, mounted() { @@ -158,20 +162,41 @@ createApp({ if (res.ok) { const data = await res.json() this.nav2Available = data.nav2_available - if (data.current_pos) { - this.navCurrentPos = data.current_pos + if (data.current_position) { + this.navCurrentPos = data.current_position } } } catch (e) {} }, async onMapClick(e) { - if (!this.mapMeta || !this.agvConnected) return + if (!this.mapMeta) { + this.mapMsg = '❌ 地图未加载' + setTimeout(() => { this.mapMsg = '' }, 3000) + return + } + if (!this.agvConnected) { + this.mapMsg = '❌ AGV 未连接，无法导航' + setTimeout(() => { this.mapMsg = '' }, 3000) + return + } const rect = e.target.getBoundingClientRect() - const px = (e.clientX - rect.left) / rect.width - const py = (e.clientY - rect.top) / rect.height + let px = (e.clientX - rect.left) / rect.width + let py = (e.clientY - rect.top) / rect.height + // 逆旋转补偿：地图 CSS transform: rotate() 后，点击坐标需反向旋转 + // 使同一物理点在不同旋转角度下返回相同的世界坐标 + const rotation = (this.mapRotation || 0) * Math.PI / 180 + if (rotation !== 0) { + const cx = px - 0.5 + const cy = py - 0.5 + const cos = Math.cos(-rotation) + const sin = Math.sin(-rotation) + px = cx * cos - cy * sin + 0.5 + py = cx * sin + cy * cos + 0.5 + } const { resolution, origin } = this.mapMeta const wx = origin[0] + px * resolution * this.mapMeta.width const wy = origin[1] + (1 - py) * resolution * this.mapMeta.height + if (!confirm(`是否导航到该坐标？\nX: ${wx.toFixed(3)}\nY: ${wy.toFixed(3)}`)) return try { const res = await fetch(API + '/api/navigate/to', { method: 'POST', @@ -254,6 +279,129 @@ createApp({ if (!angles) return '—' return angles.map(a => (a || 0).toFixed(1) + '°').join(' / ') }, + // === 点位编辑器弹窗 === + openPointEdit(ri, ci) { + const point = this.getPointAt(ri, ci) + this.editingPoint = { pointRow: ri, col: ci } + if (point && point.coords && point.coords.length >= 3) { + this.pointEditor = { x: point.coords[0], y: point.coords[1], yaw: point.coords[2] || 0 } + } else { + this.pointEditor = { x: 0, y: 0, yaw: 0 } + } + }, + closePointEdit() { + this.editingPoint = null + }, + getPointOwnerLabel(pointRow, col) { + const rows = this.missionConfig.rows || 0 + if (pointRow === 0) { + return `机器行1·正面` + } else if (pointRow >= rows) { + return `机器行${rows}·背面` + } else { + return `机器行${pointRow}·背面 + 机器行${pointRow+1}·正面` + } + }, + async loadPointFromAgv() { + try { + const res = await fetch(API + '/api/agv/position') + const data = await res.json() + if (data.ok && data.position && data.position.length >= 3) { + this.pointEditor.x = data.position[0] || 0 + this.pointEditor.y = data.position[1] || 0 + this.pointEditor.yaw = data.position[2] || 0 + } else { + alert('读取AGV位置失败') + } + } catch (e) { alert('读取AGV位置失败: ' + e.message) } + }, + async savePoint() { + if (!this.editingPoint) return + const { pointRow, col } = this.editingPoint + const coords = [this.pointEditor.x, this.pointEditor.y, this.pointEditor.yaw] + const rows = this.missionConfig.rows || 0 + // 根据点位行确定 side + const sides = [] + if (pointRow === 0) { + sides.push('front') + } else if (pointRow >= rows) { + sides.push('back') + } else { + sides.push('back') + sides.push('front') + } + try { + for (const side of sides) { + const res = await fetch(API + '/api/mission/positions', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ row: pointRow, col, side, coords, poses: [] }) + }) + const data = await res.json() + if (!data.ok) { alert(`保存失败(${side}): ` + (data.error || '')); return } + } + alert('点位已保存') + await this.loadMissionConfig() + this.closePointEdit() + } catch (e) { alert('保存失败: ' + e.message) } + }, + async navigateToPoint() { + if (!confirm(`确认导航到该点位?\nX: ${this.pointEditor.x} Y: ${this.pointEditor.y} Yaw: ${this.pointEditor.yaw}`)) return + try { + const res = await fetch(API + '/api/navigate/to', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ + x: this.pointEditor.x, + y: this.pointEditor.y, + yaw: this.pointEditor.yaw + }) + }) + const data = await res.json() + if (!data.ok) { alert('导航失败: ' + (data.error || '')) } + } catch (e) { alert('导航失败: ' + e.message) } + }, + async goToOrigin() { + if (!confirm('确认导航到原点 (0, 0, 0)?')) return + try { + const res = await fetch(API + '/api/navigate/to', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ x: 0, y: 0, yaw: 0 }) + }) + const data = await res.json() + if (data.ok) { + this.mapMsg = '✅ 已发送导航到原点' + } else { + this.mapMsg = '❌ ' + (data.error || '导航失败') + } + } catch (e) { + this.mapMsg = '❌ 导航请求失败: ' + e.message + } + setTimeout(() => { this.mapMsg = '' }, 3000) + }, + async clearPoint() { + if (!this.editingPoint) return + const { pointRow, col } = this.editingPoint + const rows = this.missionConfig.rows || 0 + const sides = pointRow === 0 ? ['front'] : pointRow >= rows ? ['back'] : ['front', 'back'] + try { + for (const side of sides) { + await fetch(API + '/api/mission/positions', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ row: pointRow, col, side, coords: [0, 0, 0], poses: [] }) + }) + } + await this.loadMissionConfig() + this.closePointEdit() + } catch (e) { alert('清空失败: ' + e.message) } + }, + canClearPoint(pointRow, col) { + const point = this.getPointAt(pointRow, col) + if (!point || !point.coords) return true + return point.coords[0] === 0 && point.coords[1] === 0 + }, // === 机型管理 === async loadAllModels() { const res = await fetch(API + '/api/models/list') @@ -344,7 +492,7 @@ createApp({ const res = await fetch(API + '/api/arm/set_angles', { method: 'POST', headers: { 'Content-Type': 'application/json' }, - body: JSON.stringify({ angles: pose.arm_angles, speed: 50 }) + body: JSON.stringify({ angles: pose.arm_angles, speed: 500 }) }) const data = await res.json() if (data.ok) { alert('姿态已应用到机械臂') } @@ -382,6 +530,8 @@ createApp({ this.missionConfig.rows = data.config.rows || 3 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) } }, @@ -413,7 +563,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() @@ -422,6 +573,47 @@ 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 applyArmInitialPose() { + if (!this.armConnected) { alert('机械臂未连接'); return } + if (!confirm('确认应用初始姿态到机械臂？')) return + try { + const res = await fetch(API + '/api/arm/set_angles', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ angles: this.armInitialPose, speed: 30 }) + }) + const data = await res.json() + if (data.ok) alert('✅ 机械臂已移动到初始姿态') + else alert('❌ 应用失败: ' + (data.error || '')) + } catch (e) { alert('应用失败: ' + e.message) } + }, async loadAllMachines() { try { const res = await fetch(API + '/api/mission/machines') @@ -465,6 +657,16 @@ createApp({ this.selectMachine(m) } }, + toggleMachine(ri, ci, event) { + if (event.target.checked) { + // 无机器 → 创建机器 + this.createMachine(ri, ci) + } else { + // 有机器 → 删除机器 + const m = this.getMachineAt(ri, ci) + if (m) this.deleteMachine(m.id) + } + }, async createMachine(ri, ci) { try { const machineId = 'm_' + ri + '_' + ci @@ -887,7 +1089,7 @@ createApp({ const res = await fetch(API + '/api/arm/set_angles', { method: 'POST', headers: {'Content-Type':'application/json'}, - body: JSON.stringify({ angles: q.joint_angles, speed: 50 }) + body: JSON.stringify({ angles: q.joint_angles, speed: 500 }) }) const data = await res.json() if (data.ok) { alert('姿态已应用到机械臂') diff --git a/agv_app/templates/setting.html b/agv_app/templates/setting.html index 00126d2..58e1d21 100644 --- a/agv_app/templates/setting.html +++ b/agv_app/templates/setting.html @@ -4,7 +4,7 @@ 设置 - AGV 拍摄系统 - +
@@ -583,7 +583,7 @@
- - + + diff --git a/agv_app/utils/mission_executor.py b/agv_app/utils/mission_executor.py index 6181a5d..b591eb6 100644 --- a/agv_app/utils/mission_executor.py +++ b/agv_app/utils/mission_executor.py @@ -185,31 +185,43 @@ class MissionExecutorV3: # 1. 生成蛇形路径 path = MissionExecutorV3._build_snake_path(rows, cols, grid) if not path: - self._log("❌ 网格中没有机器，任务终止") - self.report["error"] = "No machines in grid" + self._log("❌ 网格中没有点位，任务终止") + self.report["error"] = "No points in grid" return self._finish(0) - self.report["total"] = len(path) - self._log(f"📍 蛇形路径生成: {len(path)} 台机器") + # 统计总机器数（用于进度计算） + total_machines = 0 + for rw in grid: + for v in rw: + if v: + total_machines += 1 + self.report["total"] = total_machines + # 扫码结果缓存：正面扫到的 QR 传给背面 + qr_cache = {} - # 初始化任务列表 - self.report["tasks"] = [{ - "row": r, "col": c, - "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] + # 初始化任务列表（机器级别） + self.report["tasks"] = [] + for r in range(rows): + for c in range(cols): + if MissionExecutorV3._has_machine(grid, r, c): + self.report["tasks"].append({ + "row": r, "col": c, + "machine_id": f"m_{r}_{c}", + "label": f"{r+1}-{c+1}", + "status": "pending", + "step": "等待", + "qr_value": None, + "photos_front": 0, + "photos_back": 0, + }) + + self._log(f"📍 点位蛇形路径: {len(path)} 个点位, {total_machines} 台机器") # 任务步骤控制开关 if options is None: options = {} opt_arm_init = options.get("arm_init", True) opt_agv_move = options.get("agv_move", True) - # 机械臂初始化并入 AGV 移动：只要开移动，就先初始化机械臂 if opt_agv_move: opt_arm_init = True opt_qr_scan = options.get("qr_scan", True) @@ -222,140 +234,151 @@ class MissionExecutorV3: arm_initial_pose = mission_config.get("arm_initial_pose", [0.0] * 6) has_arm_pose = self.arm_client and any(abs(a) > 0.01 for a in arm_initial_pose) - # 2. 逐台执行 - machine_idx = 0 - while machine_idx < len(path): + # 进度统计 + max_actions = total_machines * 2 # 每台机器正面+背面 + completed_actions = 0 + + # 2. 逐点位蛇形执行 + pi = 0 + while pi < len(path): if self._stop.is_set(): self._log("⏹️ 任务已停止") break self._wait_pause() - r, c = path[machine_idx] - rl, cl = r + 1, c + 1 + pr, c = path[pi] # pr = 点位行, c = 列 - # 恢复机械臂初始姿态 - if opt_arm_init and has_arm_pose: + # 判断该点位需要做什么 + has_front = pr < rows and MissionExecutorV3._has_machine(grid, pr, c) + has_back = pr > 0 and MissionExecutorV3._has_machine(grid, pr - 1, c) + + if not has_front and not has_back: + pi += 1 + continue + + # 日志 & 步骤更新 + rl_front = pr + 1 if has_front else 0 + cl_front = c + 1 if has_front else 0 + rl_back = pr if has_back else 0 + cl_back = c + 1 if has_back else 0 + + log_parts = [] + 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})") + + # 恢复机械臂初始姿态（AGV 移动前） + if opt_arm_init and has_arm_pose and opt_agv_move: self._log(" 🦾 恢复机械臂初始姿态") try: self.arm_client.set_angles(arm_initial_pose, speed=500) - time.sleep(2) + self._wait_arm_ready(arm_initial_pose) except Exception as e: self._log(f" ⚠️ 机械臂初始化失败: {e}") - # 更新任务状态 → 正面开始 - task = self._get_task(r, c) - if task: - task["status"] = "active" - task["step"] = "正面扫码" - - 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"⚠️ 机器 {rl}-{cl} 不存在，跳过") - machine_idx += 1 - continue - - # --- 正面 --- - self._log(f"📍 机器 {rl}-{cl} 进入正面点位") - self._step(f"机器 {rl}-{cl} 正面") - - # 导航到正面点位 + # 导航到该点位的坐标 if opt_agv_move: - front_pt = self._find_point(positions, r, c, "front") - if front_pt and self._has_coords(front_pt): - if not self._navigate(front_pt, "正面"): + # 找该点位的任意有效坐标（正面/背面坐标相同） + pos = MissionExecutorV3._find_any_position(positions, pr, c) + if pos and MissionExecutorV3._has_coords(pos): + if not self._navigate(pos, f"点位({pr},{c})"): self._log(f"⚠️ 导航失败，尝试继续") - choice = self._wait_error(f"机器 {rl}-{cl} 正面导航失败") + choice = self._wait_error(f"点位({pr},{c})导航失败") if choice == "abort": break else: - self._log(f"⚠️ 无正面点位坐标") + self._log(f"⚠️ 点位({pr},{c})无有效坐标") else: - self._log(" ⏭️ 跳过AGV移动（正面）") + self._log(" ⏭️ 跳过AGV移动") - # 扫描二维码 + # --- 正面操作（机器 pr,c 的正面） --- qr_value = None - if opt_qr_scan: - qr_value = self._scan_qr_with_poses(qr_configs) - if self._stop.is_set(): - break - else: - self._log(" ⏭️ 跳过二维码识别") - - # 查机型 + 更新任务步骤 - model_name = self._lookup_model(qr_value) - self._log(f" 🏷️ 机型: {model_name}") - if task and qr_value: - task["qr_value"] = qr_value - if task: - task["step"] = "正面拍照" - - # 正面拍照 - if opt_front_photo: - model = self._find_model(models, model_name) - if model: - self._shoot(model, "front", rl, cl, qr_value or "unknown") + if has_front and not self._stop.is_set(): + self._wait_pause() + if opt_qr_scan: + qr_value = self._scan_qr_with_poses(qr_configs) + if self._stop.is_set(): + break else: - self._log(f" ⚠️ 未找到机型 {model_name}") - else: - self._log(" ⏭️ 跳过正面拍照") + self._log(" ⏭️ 跳过二维码识别（正面）") + qr_cache[(pr, c)] = qr_value - self._progress(machine_idx, 1) + task = self._get_task(pr, c) + if task and qr_value: + task["qr_value"] = qr_value + if task: + task["step"] = "正面拍照" - # --- 背面 --- - if task: - task["step"] = "背面拍照" - self._log(f"📍 机器 {rl}-{cl} 进入背面点位") - self._step(f"机器 {rl}-{cl} 背面") + model_name = self._lookup_model(qr_value) + self._log(f" 🏷️ 机型: {model_name}") - # 导航到背面点位 - # 导航到背面点位之前：恢复机械臂初始姿态 - if opt_agv_move and has_arm_pose: - self._log(" 🦾 恢复机械臂初始姿态（背面移动前）") - try: - self.arm_client.set_angles(arm_initial_pose, speed=500) - time.sleep(2) - except Exception as e: - self._log(f" ⚠️ 机械臂初始化失败: {e}") - if opt_agv_move: - back_pt = self._find_point(positions, r + 1, c, "back") - 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 + 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") + else: + self._log(f" ⚠️ 未找到机型 {model_name}") else: - self._log(f"⚠️ 无背面点位坐标") - else: - self._log(" ⏭️ 跳过AGV移动（背面）") + self._log(" ⏭️ 跳过正面拍照") + completed_actions += 1 - # 背面拍照 - if opt_back_photo: - if model: - self._shoot(model, "back", rl, cl, qr_value or "unknown") - else: - self._log(" ⏭️ 跳过背面拍照") + # --- 背面操作（机器 pr-1,c 的背面） --- + if has_back and not self._stop.is_set(): + self._wait_pause() + back_qr = qr_cache.get((pr - 1, c), "unknown") - # 标记任务完成 - if task: - task["status"] = "completed" - task["step"] = "完成" - self._progress(machine_idx, 2) + task = self._get_task(pr - 1, c) + if task: + task["step"] = "背面拍照" - # 单步执行：等待用户确认 + model_name = self._lookup_model(back_qr) + self._log(f" 🏷️ 机型(背面): {model_name}") + + if opt_back_photo and not self._stop.is_set(): + model = self._find_model(models, model_name) + if model: + self._shoot(model, "back", rl_back, cl_back, back_qr) + else: + self._log(f" ⚠️ 未找到机型 {model_name}") + else: + self._log(" ⏭️ 跳过背面拍照") + completed_actions += 1 + if task: + task["status"] = "completed" + task["step"] = "完成" + + # 更新进度 + if max_actions: + self.report["progress"] = min(int(completed_actions / max_actions * 100), 99) + + # 单步执行 if single_step and not self._stop.is_set(): - choice = self._wait_step_confirm(rl, cl) + choice = self._wait_step_confirm( + rl_front if has_front else rl_back, + cl_front if has_front else cl_back + ) if choice == "abort": break elif choice == "retry": - if task: - task["status"] = "pending" - task["step"] = "重试开始" - self._progress(machine_idx, 0) - continue # 不递增 machine_idx，重新执行 + if has_front: + task = self._get_task(pr, c) + if task: + task["status"] = "pending" + task["step"] = "重试开始" + completed_actions = max(0, completed_actions - 2) + continue - machine_idx += 1 + pi += 1 # 3. 回到出发点 if not self._stop.is_set() and opt_agv_move: @@ -391,17 +414,22 @@ class MissionExecutorV3: @staticmethod def _build_snake_path(rows: int, cols: int, grid: list) -> list: - """奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左""" + """生成点位级蛇形路径：遍历点位行 0→rows，奇数行左→右，偶数行右→左 + + 每个点位 (pr, c) 同时服务： + - 正面：机器 (pr, c)（如果 pr < rows 且 grid[pr][c] 为真） + - 背面：机器 (pr-1, c)（如果 pr > 0 且 grid[pr-1][c] 为真） + + 按此路径走完所有点位，是最短的蛇形走位，不再反复横跳。 + """ path = [] - for r in range(rows): - if r % 2 == 0: + for pr in range(rows + 1): # 点位行 = 机器行 + 1 + if pr % 2 == 0: for c in range(cols): - if MissionExecutorV3._has_machine(grid, r, c): - path.append((r, c)) + path.append((pr, c)) else: for c in range(cols - 1, -1, -1): - if MissionExecutorV3._has_machine(grid, r, c): - path.append((r, c)) + path.append((pr, c)) return path @staticmethod @@ -461,6 +489,15 @@ class MissionExecutorV3: # ==================== 点位查找 ==================== + @staticmethod + def _find_any_position(positions: list, row: int, col: int) -> Optional[dict]: + """查找点位的任意有效坐标（正面/背面坐标相同，取第一个有坐标的）""" + for side in ("front", "back"): + p = MissionExecutorV3._find_point(positions, row, col, side) + if p and MissionExecutorV3._has_coords(p): + return p + return None + @staticmethod def _find_point(positions: list, row: int, col: int, side: str) -> Optional[dict]: for p in positions: @@ -484,6 +521,25 @@ class MissionExecutorV3: # ==================== 二维码扫描 ==================== + + def _wait_arm_ready(self, target_angles: list, timeout: float = 10.0, tolerance: float = 3.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() + if ok and current_rad and len(current_rad) >= 6: + # get_angles() 返回弧度，target_angles 是角度，需转换后比较 + current_deg = [math.degrees(a) for a in current_rad] + if all(abs(current_deg[i] - target_angles[i]) < tolerance for i in range(6)): + return True + except Exception: + pass + time.sleep(0.5) + self._log(f" ⚠️ 机械臂稳定等待超时 (target={target_angles})") + return False def _scan_qr_with_poses(self, qr_configs: list) -> Optional[str]: """用二维码配置中的姿态依次尝试，逐一调整姿态+等2秒+扫码，全部失败才弹框""" if not qr_configs: @@ -502,7 +558,7 @@ class MissionExecutorV3: # 调整机械臂姿态 if self.arm_client: self.arm_client.set_angles(angles, speed=500) - time.sleep(2) # 等机械臂稳定 + 摄像头图像稳定 + self._wait_arm_ready(angles) # 读取摄像头并扫码 qr = self._decode_qr_from_arm() if qr: @@ -601,7 +657,7 @@ class MissionExecutorV3: # 调整机械臂 if self.arm_client: self.arm_client.set_angles(angles, speed=500) - time.sleep(QR_POSE_WAIT) + self._wait_arm_ready(angles) # 拍照 path = self._capture_arm_photo(row, col, side, pi + 1, qr_value)