速度调节

agv_app/app.py

@@ -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}")
     

agv_app/data/machines_config.json

@@ -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": ""
+    }
   }
 ]

agv_app/data/mission_config.json

@@ -8,9 +8,9 @@
       "col": 0,
       "side": "front",
       "coords": [
-        0.7790541397954953,
+        0.5391402360519819,
-        -1.4475345726592936,
+        -1.3221212932804989,
-        0.04652525663733687
+        -0.04968159116162075
       ],
       "poses": []
     },
@@ -19,9 +19,9 @@
       "col": 1,
       "side": "front",
       "coords": [
-        1.7721798197110892,
+        1.1801154538454173,
-        -1.4172647050697822,
+        -1.3641834306281595,
-        0.026346988400783075
+        -0.0384636372066124
       ],
       "poses": []
     },
@@ -30,9 +30,9 @@
       "col": 1,
       "side": "back",
       "coords": [
-        2.0156283932242465,
+        1.3273254588744863,
-        -3.6286459327051652,
+        -3.5287940020200854,
-        -3.1344357375846337
+        -3.11993523836094
       ],
       "poses": []
     },
@@ -41,9 +41,9 @@
       "col": 0,
       "side": "back",
       "coords": [
-        0.6542779107672178,
+        0.6499623251724095,
-        -3.610037554246379,
+        -3.634895898964233,
-        -3.139991515496645
+        -3.06371982741706
       ],
       "poses": []
     },
@@ -52,9 +52,9 @@
       "col": 2,
       "side": "front",
       "coords": [
-        3.032329928935363,
+        1.9780660285152205,
-        -1.4045725439516354,
+        -1.4118225222055494,
-        0.019396580473191316
+        -0.03933461738640764
       ],
       "poses": []
     },
@@ -63,9 +63,9 @@
       "col": 3,
       "side": "front",
       "coords": [
-        4.355614139761155,
+        2.783104887196572,
-        -1.4037408856787776,
+        -1.4531680360293173,
-        0.028879849807957422
+        -0.005407493209801511
       ],
       "poses": []
     },
@@ -74,9 +74,9 @@
       "col": 4,
       "side": "front",
       "coords": [
-        5.591022741827066,
+        3.4135017183966694,
-        -1.4041991720435099,
+        -1.463517938299615,
-        0.007950673643717402
+        -0.0022379727318056074
       ],
       "poses": []
     },
@@ -85,9 +85,9 @@
       "col": 4,
       "side": "back",
       "coords": [
-        5.855580527635482,
+        3.595502564320599,
-        -3.653374052468203,
+        -3.5861571623928663,
-        -3.1205683154447357
+        3.105599537556842
       ],
       "poses": []
     },
@@ -96,9 +96,9 @@
       "col": 4,
       "side": "front",
       "coords": [
-        5.855580527635482,
+        3.595502564320599,
-        -3.653374052468203,
+        -3.5861571623928663,
-        -3.1205683154447357
+        3.105599537556842
       ],
       "poses": []
     },
@@ -107,9 +107,9 @@
       "col": 3,
       "side": "back",
       "coords": [
-        4.639109284799733,
+        2.8436692518324937,
-        -3.6545643029563504,
+        -3.5087893361886504,
-        -3.103938599592417
+        -3.0640151322957476
       ],
       "poses": []
     },
@@ -118,9 +118,9 @@
       "col": 3,
       "side": "front",
       "coords": [
-        4.639109284799733,
+        2.8436692518324937,
-        -3.6545643029563504,
+        -3.5087893361886504,
-        -3.103938599592417
+        -3.0640151322957476
       ],
       "poses": []
     },
@@ -129,9 +129,9 @@
       "col": 2,
       "side": "back",
       "coords": [
-        3.352049221669485,
+        2.0238357078548397,
-        -3.6285597877078626,
+        -3.519588818855445,
-        3.1386114980262145
+        -3.0949553553741684
       ],
       "poses": []
     },
@@ -140,9 +140,9 @@
       "col": 2,
       "side": "front",
       "coords": [
-        3.352049221669485,
+        2.0238357078548397,
-        -3.6285597877078626,
+        -3.519588818855445,
-        3.1386114980262145
+        -3.0949553553741684
       ],
       "poses": []
     },
@@ -151,9 +151,9 @@
       "col": 1,
       "side": "front",
       "coords": [
-        2.0156283932242465,
+        1.3273254588744863,
-        -3.6286459327051652,
+        -3.5287940020200854,
-        -3.1344357375846337
+        -3.11993523836094
       ],
       "poses": []
     },
@@ -162,9 +162,9 @@
       "col": 0,
       "side": "front",
       "coords": [
-        0.6542779107672178,
+        0.6499623251724095,
-        -3.610037554246379,
+        -3.634895898964233,
-        -3.139991515496645
+        -3.06371982741706
       ],
       "poses": []
     },
@@ -173,9 +173,9 @@
       "col": 0,
       "side": "back",
       "coords": [
-        0,
+        0.39025594509020667,
-        0,
+        -5.593393651151741,
-        0
+        -0.09001000079607593
       ],
       "poses": []
     },
@@ -184,9 +184,42 @@
       "col": 1,
       "side": "back",
       "coords": [
-        0,
+        0.9989880876134289,
-        0,
+        -5.633047903271201,
-        0
+        -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": []
     }

agv_app/data/models_config.json

@@ -9,12 +9,12 @@
         "name": "front_1",
         "photo_type": "front",
         "arm_angles": [
-          46.333359,
+          -93.586541,
-          -90.759907,
+          -184.343305,
-          7.5e-05,
+          50.583239,
-          -89.156729,
+          -38.326674,
-          -15.823339,
+          -85.153333,
-          20.403326
+          20.399989
         ],
         "speed": 500,
         "description": ""
@@ -24,12 +24,12 @@
         "name": "front_2",
         "photo_type": "front",
         "arm_angles": [
-          46.333325,
+          15.859743,
-          -88.800201,
+          -6.800197,
-          0.000164,
+          -147.249916,
-          -89.1564,
+          -19.826791,
-          -15.823202,
+          168.001057,
-          20.403374
+          22.816756
         ],
         "speed": 500,
         "description": ""
@@ -39,12 +39,12 @@
         "name": "back_1",
         "photo_type": "back",
         "arm_angles": [
-          0,
+          15.860045,
-          0,
+          -161.133416,
-          0,
+          137.999016,
-          0,
+          -161.996719,
-          0,
+          168.000989,
-          0
+          15.653445
         ],
         "speed": 500,
         "description": ""
@@ -54,12 +54,27 @@
         "name": "back_2",
         "photo_type": "back",
         "arm_angles": [
-          0,
+          7.443329,
-          0,
+          -135.71653,
-          0,
+          110.665327,
-          0,
+          -161.996788,
-          0,
+          168.000941,
-          0
+          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": ""

agv_app/mission_executor.py

@@ -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,31 +189,43 @@ class MissionExecutorV3:
             # 1. 生成蛇形路径
             path = MissionExecutorV3._build_snake_path(rows, cols, grid)
             if not path:
-                self._log("❌ 网格中没有机器，任务终止")
+                self._log("❌ 网格中没有点位，任务终止")
-                self.report["error"] = "No machines in grid"
+                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"] = []
-                "row": r, "col": c,
+            for r in range(rows):
-                "machine_id": f"m_{r}_{c}",
+                for c in range(cols):
-                "label": f"{r+1}-{c+1}",
+                    if MissionExecutorV3._has_machine(grid, r, c):
-                "status": "pending",
+                        self.report["tasks"].append({
-                "step": "等待",
+                            "row": r, "col": c,
-                "qr_value": None,
+                            "machine_id": f"m_{r}_{c}",
-                "photos_front": 0,
+                            "label": f"{r+1}-{c+1}",
-                "photos_back": 0,
+                            "status": "pending",
-            } for (r, c) in path]
+                            "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,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
+            self.arm_speed = int(options.get("arm_speed", 500))
-            while machine_idx < len(path):
+            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]
+                pr, c = path[pi]  # pr = 点位行, c = 列
-                rl, cl = r + 1, c + 1
 
-                # 恢复机械臂初始姿态
+                # 判断该点位需要做什么
-                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:
+                    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})")
+
+                # 恢复机械臂初始姿态（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)
+                        self.arm_client.set_angles(arm_initial_pose, speed=self.arm_speed)
-                        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):
+                    pos = MissionExecutorV3._find_any_position(positions, pr, c)
-                        if not self._navigate(front_pt, "正面"):
+                    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:
+                if has_front and not self._stop.is_set():
-                    qr_value = self._scan_qr_with_poses(qr_configs)
+                    self._wait_pause()
-                    if self._stop.is_set():
+                    if opt_qr_scan:
-                        break
+                        qr_value = self._scan_qr_with_poses(qr_configs)
-                else:
+                        if self._stop.is_set():
-                    self._log("  ⏭️ 跳过二维码识别")
+                            break
-
-                # 查机型 + 更新任务步骤
-                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")
                     else:
-                        self._log(f"  ⚠️ 未找到机型 {model_name}")
+                        self._log("  ⏭️ 跳过二维码识别（正面）")
-                else:
+                    qr_cache[(pr, c)] = qr_value
-                    self._log("  ⏭️ 跳过正面拍照")
 
-                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"] = "正面拍照"
 
-                # --- 背面 ---
+                    model_name = self._lookup_model(qr_value)
-                if task:
+                    self._log(f"  🏷️ 机型: {model_name}")
-                    task["step"] = "背面拍照"
-                self._log(f"📍 机器 {rl}-{cl} 进入背面点位")
-                self._step(f"机器 {rl}-{cl} 背面")
 
-                # 导航到背面点位
+                    if opt_front_photo and not self._stop.is_set():
-                if opt_agv_move:
+                        model = self._find_model(models, model_name)
-                    back_pt = self._find_point(positions, r + 1, c, "back")
+                        if model:
-                    if back_pt and self._has_coords(back_pt):
+                            self._shoot(model, "front", rl_front, cl_front, qr_value or "unknown")
-                        if not self._navigate(back_pt, "背面"):
+                        else:
-                            self._log(f"⚠️ 导航失败，尝试继续")
+                            self._log(f"  ⚠️ 未找到机型 {model_name}")
-                            choice = self._wait_error(f"机器 {rl}-{cl} 背面导航失败")
-                            if choice == "abort":
-                                break
                     else:
-                        self._log(f"⚠️ 无背面点位坐标")
+                        self._log("  ⏭️ 跳过正面拍照")
-                else:
+                    completed_actions += 1
-                    self._log("  ⏭️ 跳过AGV移动（背面）")
 
-                # 背面拍照
+                # --- 背面操作（机器 pr-1,c 的背面） ---
-                if opt_back_photo:
+                if has_back and not self._stop.is_set():
-                    if model:
+                    self._wait_pause()
-                        self._shoot(model, "back", rl, cl, qr_value or "unknown")
+                    back_qr = qr_cache.get((pr - 1, c), "unknown")
-                else:
-                    self._log("  ⏭️ 跳过背面拍照")
 
-                # 标记任务完成
+                    task = self._get_task(pr - 1, c)
-                if task:
+                    if task:
-                    task["status"] = "completed"
+                        task["step"] = "背面拍照"
-                    task["step"] = "完成"
-                self._progress(machine_idx, 2)
 
-                # 单步执行：等待用户确认
+                    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:
+                        if has_front:
-                            task["status"] = "pending"
+                            task = self._get_task(pr, c)
-                            task["step"] = "重试开始"
+                            if task:
-                        self._progress(machine_idx, 0)
+                                task["status"] = "pending"
-                        continue  # 不递增 machine_idx，重新执行
+                                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:
@@ -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):
+        for pr in range(rows + 1):  # 点位行 = 机器行 + 1
-            if r % 2 == 0:
+            if pr % 2 == 0:
                 for c in range(cols):
-                    if MissionExecutorV3._has_machine(grid, r, c):
+                    path.append((pr, c))
-                        path.append((r, c))
             else:
                 for c in range(cols - 1, -1, -1):
-                    if MissionExecutorV3._has_machine(grid, r, c):
+                    path.append((pr, c))
-                        path.append((r, 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)
+                self.arm_client.set_angles(angles, speed=self.arm_speed)
-                time.sleep(QR_POSE_WAIT)
+                self._wait_arm_ready(angles)
-
+            # 读取摄像头并扫码
-            # 扫码
             qr = self._decode_qr_from_arm()
             if qr:
                 self._log(f"  ✅ 识别成功: {qr}")
                 return qr
-
+            self._log(f"  ❌ {name} 未识别到二维码")
-            time.sleep(0.3)
-
         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)
+                self.arm_client.set_angles(angles, speed=self.arm_speed)
-                time.sleep(QR_POSE_WAIT)
+                self._wait_arm_ready(angles)
 
             # 拍照
             path = self._capture_arm_photo(row, col, side, pi + 1, qr_value)

agv_app/running.html

@@ -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'">
+      <section class="card">
-        <h2>📜 实时日志</h2>
+        <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>

agv_app/running.js

@@ -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'

agv_app/static/css/style.css

@@ -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;

agv_app/static/js/running.js

@@ -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'

agv_app/style.css

@@ -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;
+}

agv_app/templates/running.html

@@ -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>

agv_app/utils/mission_executor.py

@@ -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)
 
             # 拍照