FaulknerWu/smart-inspection
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

可基本执行任务

Browse Source
This commit is contained in:
ywb
2026-05-28 13:55:55 +08:00
parent f507363c34
commit 4c096a4bd1
12 changed files with 1207 additions and 669 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/config.py
+1 -5
View File
@@ -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"
agv_app/data/machines_config.json
+162 -189
View File
@@ -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": ""
}
}
]
agv_app/data/map_config.json
+2 -2
View File
@@ -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"
}
agv_app/data/mission_config.json
+169 -95
View File
@@ -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
]
}
agv_app/data/models_config.json
+20 -12
View File
@@ -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": ""
}
]
agv_app/mission_executor.py
+279 -106
View File
@@ -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
agv_app/running.html
+76 -4
View File
@@ -4,7 +4,7 @@
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>运行监控 - AGV 拍摄系统</title>
<link rel="stylesheet" href="/static/css/style.css">
<link rel="stylesheet" href="/static/css/style.css?v=20260527b">
</head>
<body>
<div id="app">
@@ -48,6 +48,46 @@
</div>
</section>
<!-- 任务步骤控制开关 -->
<section class="card">
<h2>🎛️ 任务步骤控制</h2>
<p class="hint" style="margin-bottom:12px">关闭的步骤将在本次任务中跳过</p>
<div class="toggle-grid">
<div class="toggle-item">
<label class="toggle-switch">
<input type="checkbox" v-model="agvMoveEnabled">
<span class="toggle-slider"></span>
</label>
<span class="toggle-label">🚗 AGV移动</span>
<span class="toggle-hint">含机械臂初始化，按之字形路线移动到各点位</span>
</div>
<div class="toggle-item">
<label class="toggle-switch">
<input type="checkbox" v-model="qrScanEnabled">
<span class="toggle-slider"></span>
</label>
<span class="toggle-label">📷 识别二维码</span>
<span class="toggle-hint">调整机械臂姿态扫描二维码</span>
</div>
<div class="toggle-item">
<label class="toggle-switch">
<input type="checkbox" v-model="frontPhotoEnabled">
<span class="toggle-slider"></span>
</label>
<span class="toggle-label">📸 拍正面照</span>
<span class="toggle-hint">按机型正面姿态拍照</span>
</div>
<div class="toggle-item">
<label class="toggle-switch">
<input type="checkbox" v-model="backPhotoEnabled">
<span class="toggle-slider"></span>
</label>
<span class="toggle-label">📸 拍背面照</span>
<span class="toggle-hint">按机型背面姿态拍照</span>
</div>
</div>
</section>
<!-- 任务清单 -->
<section class="card" v-if="tasks.length > 0">
<h2>📋 任务清单 ([[ tasks.length ]] 台机器)</h2>
@@ -85,8 +125,15 @@
<!-- 实时预览 -->
<section class="card">
<h2>📷 摄像头预览</h2>
<div class="camera-full">
<img :src="previewUrl" @error="onPreviewError">
<div class="camera-dual">
<div class="camera-box">
<div class="camera-label">🎥 AGV 摄像头</div>
<img :src="agvPreviewUrl" @error="onAgvPreviewError" class="camera-img">
</div>
<div class="camera-box">
<div class="camera-label">🦾 机械臂摄像头</div>
<img :src="armPreviewUrl" @error="onArmPreviewError" class="camera-img">
</div>
</div>
</section>
@@ -113,10 +160,35 @@
</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>
<script src="/static/js/vue3.global.prod.js"></script>
<script src="/static/js/running.js"></script>
<script src="/static/js/running.js?v=20260527b"></script>
</body>
</html>
agv_app/running.js
+82 -5
View File
@@ -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'
}
}
agv_app/setting.html
+21 -114
View File
@@ -4,7 +4,7 @@
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>设置 - AGV 拍摄系统</title>
<link rel="stylesheet" href="/static/css/style.css?v=20260520h">
<link rel="stylesheet" href="/static/css/style.css?v=20260527a">
</head>
<body>
<div id="app">
@@ -12,7 +12,7 @@
<div class="logo">⚙️ 系统设置</div>
<nav class="nav">
<a href="/" class="nav-link">🏠 首页</a>
<href="/setting" class="nav-link active">⚙️ 设置</a>
<a href="/setting" class="nav-link active">⚙️ 设置</a>
<a href="/running" class="nav-link">▶️ 运行</a>
</nav>
</header>
@@ -135,7 +135,7 @@
<!-- 正面姿态 -->
<div style="padding:16px;background:#0f1923">
<h4 style="margin:0 0 12px 0;color:#388e3c">🟢 正面姿态</h4>
<div v-for="pose in m.poses.filter(p => p.photo_type === 'front')" :key="pose.id" style="background:#0f1923;padding:12px;border:1px solid #2a3441;border-radius:6px;margin-bottom:8px">
<div v-for="pose in (m.poses || []).filter(p => p.photo_type === 'front')" :key="pose.id" style="background:#0f1923;padding:12px;border:1px solid #2a3441;border-radius:6px;margin-bottom:8px">
<div style="display:flex;justify-content:space-between;align-items:center;margin-bottom:8px">
<strong>{% raw %}{{ pose.name || '正面姿态' }}{% endraw %}</strong>
<button class="btn btn-danger btn-small" @click="deletePose(m.id, pose.id)">删除</button>
@@ -182,7 +182,7 @@
<!-- 背面姿态 -->
<div style="padding:16px;background:#0d1420">
<h4 style="margin:0 0 12px 0;color:#d32f2f">🔴 背面姿态</h4>
<div v-for="pose in m.poses.filter(p => p.photo_type === 'back')" :key="pose.id" style="background:#0f1923;padding:12px;border:1px solid #2a3441;border-radius:6px;margin-bottom:8px">
<div v-for="pose in (m.poses || []).filter(p => p.photo_type === 'back')" :key="pose.id" style="background:#0f1923;padding:12px;border:1px solid #2a3441;border-radius:6px;margin-bottom:8px">
<div style="display:flex;justify-content:space-between;align-items:center;margin-bottom:8px">
<strong>{% raw %}{{ pose.name || '背面姿态' }}{% endraw %}</strong>
<button class="btn btn-danger btn-small" @click="deletePose(m.id, pose.id)">删除</button>
@@ -268,6 +268,7 @@
<button class="btn btn-primary" @click="generateGrid">🔲 生成网格</button>
<button class="btn btn-secondary" @click="saveMissionConfig">💾 保存网格</button>
<button class="btn btn-warning" @click="initPose" :disabled="initPoseLoading">📍 初始化位置</button>
<button class="btn btn-primary" @click="goToOrigin">🏠 回到原点</button>
</div>
</div>
@@ -348,116 +349,22 @@
</div>
</section>
<!-- 中：选中机器的配置 -->
<section class="card" v-if="selectedMachine && selectedMachine.front && selectedMachine.back" style="margin-top:16px">
<h2>② 点位配置 — 第{% raw %}{{ selectedMachine.row+1 }}{% endraw %}行 第{% raw %}{{ selectedMachine.col+1 }}{% endraw %}列 <button class="btn btn-small" @click="clearSelection()">← 返回</button></h2>
<!-- 正面点位 -->
<div class="machine-form">
<h3>📷 正面点位</h3>
<div class="form-row">
<div class="form-group">
<label>X 坐标</label>
<input type="number" step="0.01" v-model.number="selectedMachine.front.coords[0]" placeholder="0.00">
</div>
<div class="form-group">
<label>Y 坐标</label>
<input type="number" step="0.01" v-model.number="selectedMachine.front.coords[1]" placeholder="0.00">
</div>
<div class="form-group">
<label>Yaw (弧度)</label>
<input type="number" step="0.01" v-model.number="selectedMachine.front.coords[2]" placeholder="0.00">
</div>
<div class="form-group" style="align-self:end">
<button class="btn btn-small btn-primary" @click="readPosition('front')" :disabled="!agvConnected">📍 读取当前位置</button>
</div>
<!-- 机械臂初始姿态 -->
<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 v-if="selectedMachine.front.poses && selectedMachine.front.poses.length > 0" class="pose-list">
<h4>正面姿态 ({% raw %}{{ selectedMachine.front.poses.length }}{% endraw %} 个)</h4>
<div v-for="pose in selectedMachine.front.poses" :key="pose.id" class="pose-item">
<span class="pose-name">{% raw %}{{ pose.name }}{% endraw %}</span>
<span class="pose-angles" v-if="pose.arm_angles">角度: {% raw %}{{ formatAngles(pose.arm_angles) }}{% endraw %}</span>
<button class="btn-icon" @click="deletePose(selectedMachine.id, 'front', pose.id)">🗑️</button>
</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>
<button class="btn btn-primary" @click="applyArmInitialPose" :disabled="!armConnected" style="margin-left:6px">🎯 应用当前姿态</button>
</div>
<div class="pose-add">
<input type="text" v-model="poseForm.name" placeholder="姿态名称（如：正面全景）">
<button class="btn btn-small btn-success" @click="addPoseToMachine(selectedMachine.id, 'front')">➕ 添加姿态</button>
</div>
</div>
<!-- 背面点位 -->
<div class="machine-form" style="margin-top:16px">
<h3>📷 背面点位</h3>
<div class="form-row">
<div class="form-group">
<label>X 坐标</label>
<input type="number" step="0.01" v-model.number="selectedMachine.back.coords[0]" placeholder="0.00">
</div>
<div class="form-group">
<label>Y 坐标</label>
<input type="number" step="0.01" v-model.number="selectedMachine.back.coords[1]" placeholder="0.00">
</div>
<div class="form-group">
<label>Yaw (弧度)</label>
<input type="number" step="0.01" v-model.number="selectedMachine.back.coords[2]" placeholder="0.00">
</div>
<div class="form-group" style="align-self:end">
<button class="btn btn-small btn-primary" @click="readPosition('back')" :disabled="!agvConnected">📍 读取当前位置</button>
</div>
</div>
<!-- 背面姿态列表 -->
<div v-if="selectedMachine.back.poses && selectedMachine.back.poses.length > 0" class="pose-list">
<h4>背面姿态 ({% raw %}{{ selectedMachine.back.poses.length }}{% endraw %} 个)</h4>
<div v-for="pose in selectedMachine.back.poses" :key="pose.id" class="pose-item">
<span class="pose-name">{% raw %}{{ pose.name }}{% endraw %}</span>
<span class="pose-angles" v-if="pose.arm_angles">角度: {% raw %}{{ formatAngles(pose.arm_angles) }}{% endraw %}</span>
<button class="btn-icon" @click="deletePose(selectedMachine.id, 'back', pose.id)">🗑️</button>
</div>
</div>
<div class="pose-add">
<input type="text" v-model="poseForm.name" placeholder="姿态名称（如：背面细节）">
<button class="btn btn-small btn-success" @click="addPoseToMachine(selectedMachine.id, 'back')">➕ 添加姿态</button>
</div>
</div>
<!-- 二维码位置 -->
<div class="machine-form" style="margin-top:16px" v-if="hasQr">
<h3>🔍 二维码位置</h3>
<div class="form-row">
<div class="form-group">
<label>X 坐标</label>
<input type="number" step="0.01" :value="safeQrCoord(0)" @input="setQrCoord(0, Number($event.target.value))" placeholder="0.00">
</div>
<div class="form-group">
<label>Y 坐标</label>
<input type="number" step="0.01" :value="safeQrCoord(1)" @input="setQrCoord(1, Number($event.target.value))" placeholder="0.00">
</div>
<div class="form-group">
<label>Yaw (弧度)</label>
<input type="number" step="0.01" :value="safeQrCoord(2)" @input="setQrCoord(2, Number($event.target.value))" placeholder="0.00">
</div>
<div class="form-group" style="align-self:end">
<button class="btn btn-small btn-primary" @click="readQRPosition" :disabled="!agvConnected">📍 读取当前位置</button>
</div>
</div>
<!-- QR 扫描结果 -->
<div v-if="hasQrValue" style="margin-top:8px;padding:8px;background:#0f1923;border-radius:6px">
<span style="font-size:13px">📱 二维码值: <strong>{% raw %}{{ safeQr('qr_value') }}{% endraw %}</strong></span>
<span v-if="hasQrModelId" style="margin-left:12px;font-size:13px">🏷️ 匹配机型: <strong>{% raw %}{{ safeQrModelName() }}{% endraw %}</strong></span>
<span v-else style="margin-left:12px;font-size:13px;color:#ff9800">⚠️ 未匹配到机型</span>
</div>
<div class="btn-row" style="margin-top:8px">
<button class="btn btn-small btn-success" @click="scanQRCode(selectedMachine.id)" :disabled="!cameraOpened || qrScanning">
{% raw %}{{ qrScanning ? '扫描中...' : '📷 扫描二维码' }}{% endraw %}
</button>
</div>
</div>
<div class="btn-row" style="margin-top:16px">
<button class="btn btn-danger" @click="deleteMachine(selectedMachine.id)">🗑️ 删除此机器</button>
<button class="btn btn-secondary" @click="saveMachineCoords">💾 保存此机器配置</button>
</div>
</section>
@@ -676,7 +583,7 @@
</main>
</div>
<script src="/static/js/vue3.global.prod.js?v=20260520h"></script>
<script src="/static/js/setting.js?v=20260520h"></script>
<script src="/static/js/vue3.global.prod.js?v=20260527a"></script>
<script src="/static/js/setting.js?v=20260527a"></script>
</body>
</html>
agv_app/setting.js
+210 -8
View File
@@ -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('姿态已应用到机械臂')
agv_app/templates/setting.html
+3 -3
View File
@@ -4,7 +4,7 @@
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>设置 - AGV 拍摄系统</title>
<link rel="stylesheet" href="/static/css/style.css?v=20260527a">
<link rel="stylesheet" href="/static/css/style.css?v=20260526a">
</head>
<body>
<div id="app">
@@ -583,7 +583,7 @@
</main>
</div>
<script src="/static/js/vue3.global.prod.js?v=20260527a"></script>
<script src="/static/js/setting.js?v=20260527a"></script>
<script src="/static/js/vue3.global.prod.js?v=20260526a"></script>
<script src="/static/js/setting.js?v=20260526f"></script>
</body>
</html>
agv_app/utils/mission_executor.py
+182 -126
View File
@@ -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)