diff --git a/agv_app/__pycache__/app.cpython-312.pyc b/agv_app/__pycache__/app.cpython-312.pyc index 91b98c3..3f184e9 100644 Binary files a/agv_app/__pycache__/app.cpython-312.pyc and b/agv_app/__pycache__/app.cpython-312.pyc differ diff --git a/agv_app/app.py b/agv_app/app.py index a9d488c..fc9f86c 100644 --- a/agv_app/app.py +++ b/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}") diff --git a/agv_app/data/machines_config.json b/agv_app/data/machines_config.json index abd1625..e7b572f 100644 --- a/agv_app/data/machines_config.json +++ b/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": "" + } } ] \ No newline at end of file diff --git a/agv_app/data/mission_config.json b/agv_app/data/mission_config.json index 79f78bd..380a347 100644 --- a/agv_app/data/mission_config.json +++ b/agv_app/data/mission_config.json @@ -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": [] } diff --git a/agv_app/data/models_config.json b/agv_app/data/models_config.json index fb15d33..713ad89 100644 --- a/agv_app/data/models_config.json +++ b/agv_app/data/models_config.json @@ -9,12 +9,12 @@ "name": "front_1", "photo_type": "front", "arm_angles": [ - 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": "" diff --git a/agv_app/mission_executor.py b/agv_app/mission_executor.py index d0459e2..b77a5c6 100644 --- a/agv_app/mission_executor.py +++ b/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.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,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: + # 判断该点位需要做什么 + 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) - time.sleep(2) + 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}") - # 更新任务状态 → 正面开始 - 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: - 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: @@ -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) diff --git a/agv_app/running.html b/agv_app/running.html index a77a9d9..294122c 100644 --- a/agv_app/running.html +++ b/agv_app/running.html @@ -88,6 +88,28 @@ + +
+

🚀 速度控制

+

调节任务执行时的 AGV 和机械臂速度

+
+
+ + +
+
+ + +
+
+
+

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

@@ -114,8 +136,8 @@
-
-

📜 实时日志

+
+

📜 任务日志

[[ log ]]
等待任务开始...
diff --git a/agv_app/running.js b/agv_app/running.js index 72af4d5..e3e1852 100644 --- a/agv_app/running.js +++ b/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' diff --git a/agv_app/static/css/style.css b/agv_app/static/css/style.css index 1acf317..a28ea53 100644 --- a/agv_app/static/css/style.css +++ b/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; diff --git a/agv_app/static/js/running.js b/agv_app/static/js/running.js index 72af4d5..e3e1852 100644 --- a/agv_app/static/js/running.js +++ b/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' diff --git a/agv_app/style.css b/agv_app/style.css index ad3eda8..a28ea53 100644 --- a/agv_app/style.css +++ b/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; +} diff --git a/agv_app/templates/running.html b/agv_app/templates/running.html index a5c9fc0..294122c 100644 --- a/agv_app/templates/running.html +++ b/agv_app/templates/running.html @@ -88,6 +88,28 @@
+ +
+

🚀 速度控制

+

调节任务执行时的 AGV 和机械臂速度

+
+
+ + +
+
+ + +
+
+
+

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

diff --git a/agv_app/utils/mission_executor.py b/agv_app/utils/mission_executor.py index ec531fd..b77a5c6 100644 --- a/agv_app/utils/mission_executor.py +++ b/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) # 拍照