速度调节
This commit is contained in:
ywb
Binary file not shown.
@@ -1254,6 +1254,8 @@ def api_mission_start():
|
||||
"qr_scan": bool(data.get("qr_scan", True)),
|
||||
"front_photo": bool(data.get("front_photo", True)),
|
||||
"back_photo": bool(data.get("back_photo", True)),
|
||||
"agv_speed": float(data.get("agv_speed", 0.5)),
|
||||
"arm_speed": int(data.get("arm_speed", 500)),
|
||||
}
|
||||
print(f"[Mission] options: {options}")
|
||||
|
||||
|
||||
@@ -475,5 +475,95 @@
|
||||
"qr_value": "",
|
||||
"model_id": ""
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": "m_1_2",
|
||||
"row": 1,
|
||||
"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_1_3",
|
||||
"row": 1,
|
||||
"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_4",
|
||||
"row": 1,
|
||||
"col": 4,
|
||||
"front": {
|
||||
"coords": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
"back": {
|
||||
"coords": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
"qr": {
|
||||
"coords": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"qr_value": "",
|
||||
"model_id": ""
|
||||
}
|
||||
}
|
||||
]
|
||||
@@ -8,9 +8,9 @@
|
||||
"col": 0,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
0.7790541397954953,
|
||||
-1.4475345726592936,
|
||||
0.04652525663733687
|
||||
0.5391402360519819,
|
||||
-1.3221212932804989,
|
||||
-0.04968159116162075
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -19,9 +19,9 @@
|
||||
"col": 1,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
1.7721798197110892,
|
||||
-1.4172647050697822,
|
||||
0.026346988400783075
|
||||
1.1801154538454173,
|
||||
-1.3641834306281595,
|
||||
-0.0384636372066124
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -30,9 +30,9 @@
|
||||
"col": 1,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
2.0156283932242465,
|
||||
-3.6286459327051652,
|
||||
-3.1344357375846337
|
||||
1.3273254588744863,
|
||||
-3.5287940020200854,
|
||||
-3.11993523836094
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -41,9 +41,9 @@
|
||||
"col": 0,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
0.6542779107672178,
|
||||
-3.610037554246379,
|
||||
-3.139991515496645
|
||||
0.6499623251724095,
|
||||
-3.634895898964233,
|
||||
-3.06371982741706
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -52,9 +52,9 @@
|
||||
"col": 2,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
3.032329928935363,
|
||||
-1.4045725439516354,
|
||||
0.019396580473191316
|
||||
1.9780660285152205,
|
||||
-1.4118225222055494,
|
||||
-0.03933461738640764
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -63,9 +63,9 @@
|
||||
"col": 3,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
4.355614139761155,
|
||||
-1.4037408856787776,
|
||||
0.028879849807957422
|
||||
2.783104887196572,
|
||||
-1.4531680360293173,
|
||||
-0.005407493209801511
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -74,9 +74,9 @@
|
||||
"col": 4,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
5.591022741827066,
|
||||
-1.4041991720435099,
|
||||
0.007950673643717402
|
||||
3.4135017183966694,
|
||||
-1.463517938299615,
|
||||
-0.0022379727318056074
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -85,9 +85,9 @@
|
||||
"col": 4,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
5.855580527635482,
|
||||
-3.653374052468203,
|
||||
-3.1205683154447357
|
||||
3.595502564320599,
|
||||
-3.5861571623928663,
|
||||
3.105599537556842
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -96,9 +96,9 @@
|
||||
"col": 4,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
5.855580527635482,
|
||||
-3.653374052468203,
|
||||
-3.1205683154447357
|
||||
3.595502564320599,
|
||||
-3.5861571623928663,
|
||||
3.105599537556842
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -107,9 +107,9 @@
|
||||
"col": 3,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
4.639109284799733,
|
||||
-3.6545643029563504,
|
||||
-3.103938599592417
|
||||
2.8436692518324937,
|
||||
-3.5087893361886504,
|
||||
-3.0640151322957476
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -118,9 +118,9 @@
|
||||
"col": 3,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
4.639109284799733,
|
||||
-3.6545643029563504,
|
||||
-3.103938599592417
|
||||
2.8436692518324937,
|
||||
-3.5087893361886504,
|
||||
-3.0640151322957476
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -129,9 +129,9 @@
|
||||
"col": 2,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
3.352049221669485,
|
||||
-3.6285597877078626,
|
||||
3.1386114980262145
|
||||
2.0238357078548397,
|
||||
-3.519588818855445,
|
||||
-3.0949553553741684
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -140,9 +140,9 @@
|
||||
"col": 2,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
3.352049221669485,
|
||||
-3.6285597877078626,
|
||||
3.1386114980262145
|
||||
2.0238357078548397,
|
||||
-3.519588818855445,
|
||||
-3.0949553553741684
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -151,9 +151,9 @@
|
||||
"col": 1,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
2.0156283932242465,
|
||||
-3.6286459327051652,
|
||||
-3.1344357375846337
|
||||
1.3273254588744863,
|
||||
-3.5287940020200854,
|
||||
-3.11993523836094
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -162,9 +162,9 @@
|
||||
"col": 0,
|
||||
"side": "front",
|
||||
"coords": [
|
||||
0.6542779107672178,
|
||||
-3.610037554246379,
|
||||
-3.139991515496645
|
||||
0.6499623251724095,
|
||||
-3.634895898964233,
|
||||
-3.06371982741706
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -173,9 +173,9 @@
|
||||
"col": 0,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
0.39025594509020667,
|
||||
-5.593393651151741,
|
||||
-0.09001000079607593
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
@@ -184,9 +184,42 @@
|
||||
"col": 1,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
0,
|
||||
0,
|
||||
0
|
||||
0.9989880876134289,
|
||||
-5.633047903271201,
|
||||
-0.08518177305398167
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
{
|
||||
"row": 2,
|
||||
"col": 2,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
1.7493440267548326,
|
||||
-5.7036971258959746,
|
||||
-0.10505541857885684
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
{
|
||||
"row": 2,
|
||||
"col": 3,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
2.407336669431407,
|
||||
-5.76958512207572,
|
||||
-0.10251877401062247
|
||||
],
|
||||
"poses": []
|
||||
},
|
||||
{
|
||||
"row": 2,
|
||||
"col": 4,
|
||||
"side": "back",
|
||||
"coords": [
|
||||
3.132062476985721,
|
||||
-5.850166571217611,
|
||||
-0.09127007182804729
|
||||
],
|
||||
"poses": []
|
||||
}
|
||||
|
||||
@@ -9,12 +9,12 @@
|
||||
"name": "front_1",
|
||||
"photo_type": "front",
|
||||
"arm_angles": [
|
||||
46.333359,
|
||||
-90.759907,
|
||||
7.5e-05,
|
||||
-89.156729,
|
||||
-15.823339,
|
||||
20.403326
|
||||
-93.586541,
|
||||
-184.343305,
|
||||
50.583239,
|
||||
-38.326674,
|
||||
-85.153333,
|
||||
20.399989
|
||||
],
|
||||
"speed": 500,
|
||||
"description": ""
|
||||
@@ -24,12 +24,12 @@
|
||||
"name": "front_2",
|
||||
"photo_type": "front",
|
||||
"arm_angles": [
|
||||
46.333325,
|
||||
-88.800201,
|
||||
0.000164,
|
||||
-89.1564,
|
||||
-15.823202,
|
||||
20.403374
|
||||
15.859743,
|
||||
-6.800197,
|
||||
-147.249916,
|
||||
-19.826791,
|
||||
168.001057,
|
||||
22.816756
|
||||
],
|
||||
"speed": 500,
|
||||
"description": ""
|
||||
@@ -39,12 +39,12 @@
|
||||
"name": "back_1",
|
||||
"photo_type": "back",
|
||||
"arm_angles": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
15.860045,
|
||||
-161.133416,
|
||||
137.999016,
|
||||
-161.996719,
|
||||
168.000989,
|
||||
15.653445
|
||||
],
|
||||
"speed": 500,
|
||||
"description": ""
|
||||
@@ -54,12 +54,27 @@
|
||||
"name": "back_2",
|
||||
"photo_type": "back",
|
||||
"arm_angles": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
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": ""
|
||||
|
||||
+181
-98
@@ -107,6 +107,10 @@ class MissionExecutorV3:
|
||||
# Nav2 导航器（直接使用 rclpy BasicNavigator API，比 subprocess 更可靠）
|
||||
self._nav = Nav2Navigator()
|
||||
|
||||
# 速度控制（默认值，可在 execute_mission 时覆写）
|
||||
self.arm_speed = 500
|
||||
self.agv_speed = 0.5
|
||||
|
||||
# ==================== 连接 ====================
|
||||
|
||||
def connect_all(self) -> Dict[str, bool]:
|
||||
@@ -185,15 +189,26 @@ 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"] = [{
|
||||
# 初始化任务列表（机器级别）
|
||||
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}",
|
||||
@@ -202,14 +217,15 @@ class MissionExecutorV3:
|
||||
"qr_value": None,
|
||||
"photos_front": 0,
|
||||
"photos_back": 0,
|
||||
} for (r, c) in path]
|
||||
})
|
||||
|
||||
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,132 +238,161 @@ 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):
|
||||
# 速度控制（从前端传入）
|
||||
self.arm_speed = int(options.get("arm_speed", 500))
|
||||
self.agv_speed = float(options.get("agv_speed", 0.5))
|
||||
self._log(f"🚀 AGV速度={self.agv_speed:.1f}m/s, 机械臂速度={self.arm_speed}")
|
||||
|
||||
# 设置 Nav2 导航速度（仅在任务开始时设一次）
|
||||
if opt_agv_move:
|
||||
self._set_nav_speed()
|
||||
|
||||
# 进度统计
|
||||
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:
|
||||
self._log(" 🦾 恢复机械臂初始姿态")
|
||||
try:
|
||||
self.arm_client.set_angles(arm_initial_pose, speed=500)
|
||||
time.sleep(2)
|
||||
except Exception as e:
|
||||
self._log(f" ⚠️ 机械臂初始化失败: {e}")
|
||||
# 判断该点位需要做什么
|
||||
has_front = pr < rows and MissionExecutorV3._has_machine(grid, pr, c)
|
||||
has_back = pr > 0 and MissionExecutorV3._has_machine(grid, pr - 1, c)
|
||||
|
||||
# 更新任务状态 → 正面开始
|
||||
task = self._get_task(r, c)
|
||||
if not has_front and not has_back:
|
||||
self._log(f"📍 点位 ({pr},{c}) → 空位")
|
||||
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})")
|
||||
|
||||
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
|
||||
# 恢复机械臂初始姿态（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=self.arm_speed)
|
||||
self._wait_arm_ready(arm_initial_pose)
|
||||
except Exception as e:
|
||||
self._log(f" ⚠️ 机械臂初始化失败: {e}")
|
||||
|
||||
# --- 正面 ---
|
||||
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 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(" ⏭️ 跳过二维码识别")
|
||||
self._log(" ⏭️ 跳过二维码识别（正面）")
|
||||
qr_cache[(pr, c)] = qr_value
|
||||
|
||||
# 查机型 + 更新任务步骤
|
||||
model_name = self._lookup_model(qr_value)
|
||||
self._log(f" 🏷️ 机型: {model_name}")
|
||||
task = self._get_task(pr, c)
|
||||
if task and qr_value:
|
||||
task["qr_value"] = qr_value
|
||||
if task:
|
||||
task["step"] = "正面拍照"
|
||||
|
||||
# 正面拍照
|
||||
if opt_front_photo:
|
||||
model_name = self._lookup_model(qr_value)
|
||||
self._log(f" 🏷️ 机型: {model_name}")
|
||||
|
||||
if opt_front_photo and not self._stop.is_set():
|
||||
model = self._find_model(models, model_name)
|
||||
if model:
|
||||
self._shoot(model, "front", rl, cl, qr_value or "unknown")
|
||||
self._shoot(model, "front", rl_front, cl_front, qr_value or "unknown")
|
||||
else:
|
||||
self._log(f" ⚠️ 未找到机型 {model_name}")
|
||||
else:
|
||||
self._log(" ⏭️ 跳过正面拍照")
|
||||
completed_actions += 1
|
||||
|
||||
self._progress(machine_idx, 1)
|
||||
# --- 背面操作（机器 pr-1,c 的背面） ---
|
||||
if has_back and not self._stop.is_set():
|
||||
self._wait_pause()
|
||||
back_qr = qr_cache.get((pr - 1, c), "unknown")
|
||||
|
||||
# --- 背面 ---
|
||||
task = self._get_task(pr - 1, c)
|
||||
if task:
|
||||
task["step"] = "背面拍照"
|
||||
self._log(f"📍 机器 {rl}-{cl} 进入背面点位")
|
||||
self._step(f"机器 {rl}-{cl} 背面")
|
||||
|
||||
# 导航到背面点位
|
||||
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(" ⏭️ 跳过AGV移动（背面）")
|
||||
model_name = self._lookup_model(back_qr)
|
||||
self._log(f" 🏷️ 机型(背面): {model_name}")
|
||||
|
||||
# 背面拍照
|
||||
if opt_back_photo:
|
||||
if opt_back_photo and not self._stop.is_set():
|
||||
model = self._find_model(models, model_name)
|
||||
if model:
|
||||
self._shoot(model, "back", rl, cl, qr_value or "unknown")
|
||||
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"] = "完成"
|
||||
self._progress(machine_idx, 2)
|
||||
|
||||
# 单步执行：等待用户确认
|
||||
# 更新进度
|
||||
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 has_front:
|
||||
task = self._get_task(pr, c)
|
||||
if task:
|
||||
task["status"] = "pending"
|
||||
task["step"] = "重试开始"
|
||||
self._progress(machine_idx, 0)
|
||||
continue # 不递增 machine_idx，重新执行
|
||||
completed_actions = max(0, completed_actions - 2)
|
||||
continue
|
||||
|
||||
machine_idx += 1
|
||||
pi += 1
|
||||
|
||||
# 3. 回到出发点
|
||||
if not self._stop.is_set() and opt_agv_move:
|
||||
@@ -383,17 +428,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
|
||||
@@ -453,6 +503,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:
|
||||
@@ -467,6 +526,19 @@ class MissionExecutorV3:
|
||||
|
||||
# ==================== 导航 ====================
|
||||
|
||||
def _set_nav_speed(self) -> bool:
|
||||
"""动态设置 Nav2 控制器最大速度参数"""
|
||||
try:
|
||||
# 尝试设置 controller_server 的线速度参数
|
||||
vel = self.agv_speed
|
||||
cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 param set /controller_server FollowPath.desired_linear_vel {vel:.2f} 2>/dev/null || true'"
|
||||
subprocess.run(cmd, shell=True, timeout=5, capture_output=True)
|
||||
self._log(f" 🚀 AGV 速度设为 {vel:.1f} m/s")
|
||||
return True
|
||||
except Exception as e:
|
||||
logger.warning(f"设置 AGV 速度失败: {e}")
|
||||
return False
|
||||
|
||||
def _navigate(self, point: dict, label: str) -> bool:
|
||||
coords = point["coords"]
|
||||
x, y = float(coords[0]), float(coords[1])
|
||||
@@ -476,38 +548,49 @@ class MissionExecutorV3:
|
||||
|
||||
# ==================== 二维码扫描 ====================
|
||||
|
||||
|
||||
def _wait_arm_ready(self, target_angles: list, timeout: float = 15.0, tolerance: float = 2.0) -> bool:
|
||||
"""等待机械臂稳定到目标角度（±tolerance 度），超时返回 False"""
|
||||
if not self.arm_client:
|
||||
return True
|
||||
deadline = time.time() + timeout
|
||||
while time.time() < deadline:
|
||||
try:
|
||||
ok, current = self.arm_client.get_angles()
|
||||
if ok and current and len(current) >= 6:
|
||||
# get_angles() 返回角度（度），与 target_angles 直接比较
|
||||
if all(abs(current[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:
|
||||
self._log(f" ⚠️ 无二维码配置")
|
||||
return self._request_manual_qr()
|
||||
|
||||
self._log(f" 🔍 尝试 {len(qr_configs)} 个二维码姿态...")
|
||||
for i, qc in enumerate(qr_configs):
|
||||
if self._stop.is_set():
|
||||
return None
|
||||
self._wait_pause()
|
||||
|
||||
angles = qc.get("joint_angles", [])
|
||||
if not angles or len(angles) < 6:
|
||||
continue
|
||||
|
||||
name = qc.get("name", f"姿态{i+1}")
|
||||
self._log(f" [{i+1}/{len(qr_configs)}] {name}")
|
||||
|
||||
# 调整机械臂
|
||||
# 调整机械臂姿态
|
||||
if self.arm_client:
|
||||
self.arm_client.set_angles(angles, speed=500)
|
||||
time.sleep(QR_POSE_WAIT)
|
||||
|
||||
# 扫码
|
||||
self.arm_client.set_angles(angles, speed=self.arm_speed)
|
||||
self._wait_arm_ready(angles)
|
||||
# 读取摄像头并扫码
|
||||
qr = self._decode_qr_from_arm()
|
||||
if qr:
|
||||
self._log(f" ✅ 识别成功: {qr}")
|
||||
return qr
|
||||
|
||||
time.sleep(0.3)
|
||||
|
||||
self._log(f" ❌ {name} 未识别到二维码")
|
||||
self._log(f" ⚠️ 全部 {len(qr_configs)} 个姿态均未识别到二维码")
|
||||
return self._request_manual_qr()
|
||||
|
||||
@@ -599,8 +682,8 @@ class MissionExecutorV3:
|
||||
|
||||
# 调整机械臂
|
||||
if self.arm_client:
|
||||
self.arm_client.set_angles(angles, speed=500)
|
||||
time.sleep(QR_POSE_WAIT)
|
||||
self.arm_client.set_angles(angles, speed=self.arm_speed)
|
||||
self._wait_arm_ready(angles)
|
||||
|
||||
# 拍照
|
||||
path = self._capture_arm_photo(row, col, side, pi + 1, qr_value)
|
||||
|
||||
+24
-2
@@ -88,6 +88,28 @@
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 速度控制 -->
|
||||
<section class="card">
|
||||
<h2>🚀 速度控制</h2>
|
||||
<p class="hint" style="margin-bottom:12px">调节任务执行时的 AGV 和机械臂速度</p>
|
||||
<div class="speed-panel">
|
||||
<div class="speed-row">
|
||||
<label class="speed-label">
|
||||
<span>🚗 AGV 移动速度</span>
|
||||
<span class="speed-val">[[ agvSpeed.toFixed(1) ]] m/s</span>
|
||||
</label>
|
||||
<input type="range" class="speed-slider" min="0.1" max="1.0" step="0.1" v-model.number="agvSpeed">
|
||||
</div>
|
||||
<div class="speed-row">
|
||||
<label class="speed-label">
|
||||
<span>🦾 机械臂速度</span>
|
||||
<span class="speed-val">[[ armSpeed ]]</span>
|
||||
</label>
|
||||
<input type="range" class="speed-slider" min="100" max="1000" step="50" v-model.number="armSpeed">
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 任务清单 -->
|
||||
<section class="card" v-if="tasks.length > 0">
|
||||
<h2>📋 任务清单 ([[ tasks.length ]] 台机器)</h2>
|
||||
@@ -114,8 +136,8 @@
|
||||
</section>
|
||||
|
||||
<!-- 实时日志 -->
|
||||
<section class="card" v-if="missionState === 'running' || missionState === 'waiting_qr'">
|
||||
<h2>📜 实时日志</h2>
|
||||
<section class="card">
|
||||
<h2>📜 任务日志</h2>
|
||||
<div class="log-box" ref="logBox">
|
||||
<div v-for="(log, i) in logs" :key="i" class="log-line">[[ log ]]</div>
|
||||
<div v-if="logs.length === 0" class="log-empty">等待任务开始...</div>
|
||||
|
||||
@@ -26,6 +26,9 @@ createApp({
|
||||
qrScanEnabled: true,
|
||||
frontPhotoEnabled: true,
|
||||
backPhotoEnabled: true,
|
||||
// 速度控制
|
||||
agvSpeed: 0.5,
|
||||
armSpeed: 500,
|
||||
}
|
||||
},
|
||||
computed: {
|
||||
@@ -124,6 +127,8 @@ createApp({
|
||||
qr_scan: this.qrScanEnabled,
|
||||
front_photo: this.frontPhotoEnabled,
|
||||
back_photo: this.backPhotoEnabled,
|
||||
agv_speed: this.agvSpeed,
|
||||
arm_speed: this.armSpeed,
|
||||
})
|
||||
})
|
||||
this.missionState = 'running'
|
||||
|
||||
@@ -385,6 +385,61 @@ a:hover { text-decoration: underline; }
|
||||
color: #4fc3f7;
|
||||
}
|
||||
|
||||
/* 运行页速度控制面板 */
|
||||
.speed-panel {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 16px;
|
||||
}
|
||||
.speed-row {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 6px;
|
||||
}
|
||||
.speed-label {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
font-size: 14px;
|
||||
color: #b0c4de;
|
||||
}
|
||||
.speed-val {
|
||||
font-weight: bold;
|
||||
font-size: 15px;
|
||||
color: #4fc3f7;
|
||||
min-width: 80px;
|
||||
text-align: right;
|
||||
}
|
||||
.speed-slider {
|
||||
-webkit-appearance: none;
|
||||
appearance: none;
|
||||
width: 100%;
|
||||
height: 8px;
|
||||
background: #1a2d3d;
|
||||
border-radius: 4px;
|
||||
outline: none;
|
||||
cursor: pointer;
|
||||
}
|
||||
.speed-slider::-webkit-slider-thumb {
|
||||
-webkit-appearance: none;
|
||||
appearance: none;
|
||||
width: 22px;
|
||||
height: 22px;
|
||||
border-radius: 50%;
|
||||
background: #4fc3f7;
|
||||
cursor: pointer;
|
||||
border: 2px solid #0f1923;
|
||||
box-shadow: 0 0 6px rgba(79, 195, 247, 0.4);
|
||||
}
|
||||
.speed-slider::-moz-range-thumb {
|
||||
width: 22px;
|
||||
height: 22px;
|
||||
border-radius: 50%;
|
||||
background: #4fc3f7;
|
||||
cursor: pointer;
|
||||
border: 2px solid #0f1923;
|
||||
}
|
||||
|
||||
/* 双摄像头预览布局 */
|
||||
.camera-row {
|
||||
display: grid;
|
||||
|
||||
@@ -26,6 +26,9 @@ createApp({
|
||||
qrScanEnabled: true,
|
||||
frontPhotoEnabled: true,
|
||||
backPhotoEnabled: true,
|
||||
// 速度控制
|
||||
agvSpeed: 0.5,
|
||||
armSpeed: 500,
|
||||
}
|
||||
},
|
||||
computed: {
|
||||
@@ -124,6 +127,8 @@ createApp({
|
||||
qr_scan: this.qrScanEnabled,
|
||||
front_photo: this.frontPhotoEnabled,
|
||||
back_photo: this.backPhotoEnabled,
|
||||
agv_speed: this.agvSpeed,
|
||||
arm_speed: this.armSpeed,
|
||||
})
|
||||
})
|
||||
this.missionState = 'running'
|
||||
|
||||
@@ -385,6 +385,61 @@ a:hover { text-decoration: underline; }
|
||||
color: #4fc3f7;
|
||||
}
|
||||
|
||||
/* 运行页速度控制面板 */
|
||||
.speed-panel {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 16px;
|
||||
}
|
||||
.speed-row {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 6px;
|
||||
}
|
||||
.speed-label {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
font-size: 14px;
|
||||
color: #b0c4de;
|
||||
}
|
||||
.speed-val {
|
||||
font-weight: bold;
|
||||
font-size: 15px;
|
||||
color: #4fc3f7;
|
||||
min-width: 80px;
|
||||
text-align: right;
|
||||
}
|
||||
.speed-slider {
|
||||
-webkit-appearance: none;
|
||||
appearance: none;
|
||||
width: 100%;
|
||||
height: 8px;
|
||||
background: #1a2d3d;
|
||||
border-radius: 4px;
|
||||
outline: none;
|
||||
cursor: pointer;
|
||||
}
|
||||
.speed-slider::-webkit-slider-thumb {
|
||||
-webkit-appearance: none;
|
||||
appearance: none;
|
||||
width: 22px;
|
||||
height: 22px;
|
||||
border-radius: 50%;
|
||||
background: #4fc3f7;
|
||||
cursor: pointer;
|
||||
border: 2px solid #0f1923;
|
||||
box-shadow: 0 0 6px rgba(79, 195, 247, 0.4);
|
||||
}
|
||||
.speed-slider::-moz-range-thumb {
|
||||
width: 22px;
|
||||
height: 22px;
|
||||
border-radius: 50%;
|
||||
background: #4fc3f7;
|
||||
cursor: pointer;
|
||||
border: 2px solid #0f1923;
|
||||
}
|
||||
|
||||
/* 双摄像头预览布局 */
|
||||
.camera-row {
|
||||
display: grid;
|
||||
@@ -912,3 +967,79 @@ a:hover { text-decoration: underline; }
|
||||
0%, 100% { opacity: 1; }
|
||||
50% { opacity: 0.3; }
|
||||
}
|
||||
|
||||
/* ========== 运行页双摄像头 ========== */
|
||||
.camera-dual {
|
||||
display: grid;
|
||||
grid-template-columns: 1fr 1fr;
|
||||
gap: 16px;
|
||||
margin-top: 12px;
|
||||
}
|
||||
|
||||
/* ========== 任务步骤控制开关 ========== */
|
||||
.toggle-grid {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 12px;
|
||||
}
|
||||
.toggle-item {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 12px;
|
||||
padding: 10px 14px;
|
||||
background: #0a0a0a;
|
||||
border: 1px solid #1a1a1a;
|
||||
border-radius: 8px;
|
||||
transition: border-color 0.2s;
|
||||
}
|
||||
.toggle-item:hover {
|
||||
border-color: #333;
|
||||
}
|
||||
.toggle-switch {
|
||||
position: relative;
|
||||
display: inline-block;
|
||||
width: 44px;
|
||||
height: 24px;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
.toggle-switch input {
|
||||
opacity: 0;
|
||||
width: 0;
|
||||
height: 0;
|
||||
}
|
||||
.toggle-slider {
|
||||
position: absolute;
|
||||
cursor: pointer;
|
||||
top: 0; left: 0; right: 0; bottom: 0;
|
||||
background-color: #333;
|
||||
border-radius: 24px;
|
||||
transition: 0.3s;
|
||||
}
|
||||
.toggle-slider:before {
|
||||
position: absolute;
|
||||
content: "";
|
||||
height: 18px;
|
||||
width: 18px;
|
||||
left: 3px;
|
||||
bottom: 3px;
|
||||
background-color: #888;
|
||||
border-radius: 50%;
|
||||
transition: 0.3s;
|
||||
}
|
||||
.toggle-switch input:checked + .toggle-slider {
|
||||
background-color: #2e7d32;
|
||||
}
|
||||
.toggle-switch input:checked + .toggle-slider:before {
|
||||
transform: translateX(20px);
|
||||
background-color: #4caf50;
|
||||
}
|
||||
.toggle-label {
|
||||
font-size: 14px;
|
||||
font-weight: 600;
|
||||
color: #e0e0e0;
|
||||
min-width: 140px;
|
||||
}
|
||||
.toggle-hint {
|
||||
font-size: 12px;
|
||||
color: #666;
|
||||
}
|
||||
|
||||
@@ -88,6 +88,28 @@
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 速度控制 -->
|
||||
<section class="card">
|
||||
<h2>🚀 速度控制</h2>
|
||||
<p class="hint" style="margin-bottom:12px">调节任务执行时的 AGV 和机械臂速度</p>
|
||||
<div class="speed-panel">
|
||||
<div class="speed-row">
|
||||
<label class="speed-label">
|
||||
<span>🚗 AGV 移动速度</span>
|
||||
<span class="speed-val">[[ agvSpeed.toFixed(1) ]] m/s</span>
|
||||
</label>
|
||||
<input type="range" class="speed-slider" min="0.1" max="1.0" step="0.1" v-model.number="agvSpeed">
|
||||
</div>
|
||||
<div class="speed-row">
|
||||
<label class="speed-label">
|
||||
<span>🦾 机械臂速度</span>
|
||||
<span class="speed-val">[[ armSpeed ]]</span>
|
||||
</label>
|
||||
<input type="range" class="speed-slider" min="100" max="1000" step="50" v-model.number="armSpeed">
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 任务清单 -->
|
||||
<section class="card" v-if="tasks.length > 0">
|
||||
<h2>📋 任务清单 ([[ tasks.length ]] 台机器)</h2>
|
||||
|
||||
@@ -107,6 +107,10 @@ class MissionExecutorV3:
|
||||
# Nav2 导航器（直接使用 rclpy BasicNavigator API，比 subprocess 更可靠）
|
||||
self._nav = Nav2Navigator()
|
||||
|
||||
# 速度控制（默认值，可在 execute_mission 时覆写）
|
||||
self.arm_speed = 500
|
||||
self.agv_speed = 0.5
|
||||
|
||||
# ==================== 连接 ====================
|
||||
|
||||
def connect_all(self) -> Dict[str, bool]:
|
||||
@@ -234,6 +238,15 @@ 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)
|
||||
|
||||
# 速度控制（从前端传入）
|
||||
self.arm_speed = int(options.get("arm_speed", 500))
|
||||
self.agv_speed = float(options.get("agv_speed", 0.5))
|
||||
self._log(f"🚀 AGV速度={self.agv_speed:.1f}m/s, 机械臂速度={self.arm_speed}")
|
||||
|
||||
# 设置 Nav2 导航速度（仅在任务开始时设一次）
|
||||
if opt_agv_move:
|
||||
self._set_nav_speed()
|
||||
|
||||
# 进度统计
|
||||
max_actions = total_machines * 2 # 每台机器正面+背面
|
||||
completed_actions = 0
|
||||
@@ -282,7 +295,7 @@ class MissionExecutorV3:
|
||||
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)
|
||||
self.arm_client.set_angles(arm_initial_pose, speed=self.arm_speed)
|
||||
self._wait_arm_ready(arm_initial_pose)
|
||||
except Exception as e:
|
||||
self._log(f" ⚠️ 机械臂初始化失败: {e}")
|
||||
@@ -513,6 +526,19 @@ class MissionExecutorV3:
|
||||
|
||||
# ==================== 导航 ====================
|
||||
|
||||
def _set_nav_speed(self) -> bool:
|
||||
"""动态设置 Nav2 控制器最大速度参数"""
|
||||
try:
|
||||
# 尝试设置 controller_server 的线速度参数
|
||||
vel = self.agv_speed
|
||||
cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 param set /controller_server FollowPath.desired_linear_vel {vel:.2f} 2>/dev/null || true'"
|
||||
subprocess.run(cmd, shell=True, timeout=5, capture_output=True)
|
||||
self._log(f" 🚀 AGV 速度设为 {vel:.1f} m/s")
|
||||
return True
|
||||
except Exception as e:
|
||||
logger.warning(f"设置 AGV 速度失败: {e}")
|
||||
return False
|
||||
|
||||
def _navigate(self, point: dict, label: str) -> bool:
|
||||
coords = point["coords"]
|
||||
x, y = float(coords[0]), float(coords[1])
|
||||
@@ -557,7 +583,7 @@ class MissionExecutorV3:
|
||||
self._log(f" [{i+1}/{len(qr_configs)}] {name}")
|
||||
# 调整机械臂姿态
|
||||
if self.arm_client:
|
||||
self.arm_client.set_angles(angles, speed=500)
|
||||
self.arm_client.set_angles(angles, speed=self.arm_speed)
|
||||
self._wait_arm_ready(angles)
|
||||
# 读取摄像头并扫码
|
||||
qr = self._decode_qr_from_arm()
|
||||
@@ -656,7 +682,7 @@ class MissionExecutorV3:
|
||||
|
||||
# 调整机械臂
|
||||
if self.arm_client:
|
||||
self.arm_client.set_angles(angles, speed=500)
|
||||
self.arm_client.set_angles(angles, speed=self.arm_speed)
|
||||
self._wait_arm_ready(angles)
|
||||
|
||||
# 拍照
|
||||
|
||||
Reference in New Issue
Block a user