From 3d892bfaf6bbb94cf2079b2da7ba4a04522d2288 Mon Sep 17 00:00:00 2001 From: ywb <347742090@qq.com> Date: Fri, 29 May 2026 23:44:25 +0800 Subject: [PATCH] - --- agv_app/data/models_config.json | 53 ----------------------------- agv_app/utils/mission_executor.py | 56 ++++++++++++++++++++----------- 2 files changed, 36 insertions(+), 73 deletions(-) diff --git a/agv_app/data/models_config.json b/agv_app/data/models_config.json index 713ad89..2dc2ddc 100644 --- a/agv_app/data/models_config.json +++ b/agv_app/data/models_config.json @@ -19,21 +19,6 @@ "speed": 500, "description": "" }, - { - "id": "pose_1778767312", - "name": "front_2", - "photo_type": "front", - "arm_angles": [ - 15.859743, - -6.800197, - -147.249916, - -19.826791, - 168.001057, - 22.816756 - ], - "speed": 500, - "description": "" - }, { "id": "pose_1778767323", "name": "back_1", @@ -48,47 +33,9 @@ ], "speed": 500, "description": "" - }, - { - "id": "pose_1778767325", - "name": "back_2", - "photo_type": "back", - "arm_angles": [ - 7.443329, - -135.71653, - 110.665327, - -161.996788, - 168.000941, - 15.653383 - ], - "speed": 500, - "description": "" - }, - { - "id": "pose_1779955047", - "name": "font_3", - "photo_type": "front", - "arm_angles": [ - 15.860004, - -161.133368, - 137.998968, - -161.996761, - 168.00114, - 15.65326 - ], - "speed": 500, - "description": "" } ], "description": "", "notes": "" - }, - { - "id": "m_1779286436", - "name": "m2", - "serial_prefix": "", - "poses": [], - "description": "", - "notes": "" } ] \ No newline at end of file diff --git a/agv_app/utils/mission_executor.py b/agv_app/utils/mission_executor.py index 824b904..3cbfdf1 100644 --- a/agv_app/utils/mission_executor.py +++ b/agv_app/utils/mission_executor.py @@ -462,11 +462,23 @@ class MissionExecutorV3: pi += 1 - # 3. 回到出发点 + # 3. 回到出发点（必须成功返回才算结束） 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) + max_retry = 3 + 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("⏭️ 跳过返回出发点") @@ -642,10 +654,10 @@ class MissionExecutorV3: if not qr_configs: self._log(f" ⚠️ 无二维码配置") return self._request_manual_qr() - # 判断是否需要反转关节角度 - invert = (machine_row % 2 == 1) - if invert: - self._log(f" 🔄 扫码：AGV朝向相反，关节角度取反") + # 判断是否需要镜像关节角度（AGV 朝向相反时，只镜像 J1） + mirror = (machine_row % 2 == 1) + if mirror: + self._log(f" 🔄 扫码：AGV朝向相反，关节角度镜像（仅 J1 取反）") self._log(f" 🔍 尝试 {len(qr_configs)} 个二维码姿态...") for i, qc in enumerate(qr_configs): if self._stop.is_set(): @@ -654,11 +666,12 @@ class MissionExecutorV3: angles = qc.get("joint_angles", []) if not angles or len(angles) < 6: continue - # 需要时反转关节角度 - if invert: - angles = [-a for a in angles] + # 需要时镜像关节角度（只取反 J1） + if mirror: + angles = angles.copy() # 避免修改原数据 + angles[0] = -angles[0] name = qc.get("name", f"姿态{i+1}") - self._log(f" [{i+1}/{len(qr_configs)}] {name}{' (反转)' if invert else ''}") + self._log(f" [{i+1}/{len(qr_configs)}] {name}{'(镜像)' if mirror else ''}") # 调整机械臂姿态 if self.arm_client: self.arm_client.set_angles(angles, speed=self.arm_speed) @@ -740,12 +753,14 @@ class MissionExecutorV3: 蛇形路径下，AGV 朝向在奇数行(1,3,5)时与出发时相反， 此时机器的正面/背面朝向与标定相反，需要把所有关节角度取反 """ - # 判断是否需要反转关节角度（AGV 朝向与出发时相反） - invert = (machine_row % 2 == 1) # 机器行是奇数(1,3,5...)时，AGV从对面来，需反转 + # 判断是否需要镜像关节角度（AGV 朝向与出发时相反，仅镜像 J1） + mirror = (machine_row % 2 == 1) # 机器行是奇数(1,3,5...)时，AGV从对面来，需镜像 side_label = "正面" if side == "front" else "背面" - if invert: - self._log(f" 🔄 {side_label}拍摄：AGV朝向相反，关节角度取反") - poses = model.get("poses", []) if side == "front" else model.get("poses_back", []) + if mirror: + self._log(f" 🔄 {side_label}拍摄：AGV朝向相反，关节角度镜像（仅 J1 取反）") + # 修复：统一从 poses 中按 photo_type 过滤 + all_poses = model.get("poses", []) + poses = [p for p in all_poses if p.get("photo_type") == side] if not poses: self._log(f" ⚠️ 机型无{side_label}姿态配置") return @@ -761,11 +776,12 @@ class MissionExecutorV3: self._log(f" 跳过 {pose.get('name', f'姿态{pi+1}')}: 无效角度") continue - # 需要时对所有关节角度取反 - if invert: - angles = [-a for a in angles] + # 需要时镜像关节角度（仅取反 J1） + if mirror: + angles = angles.copy() # 避免修改原数据 + angles[0] = -angles[0] name = pose.get("name", f"{side_label}-{pi+1}") - self._log(f" 🎯 {name}{' (反转)' if invert else ''}") + self._log(f" 🎯 {name}{'(镜像)' if mirror else ''}") # 调整机械臂 if self.arm_client: