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

速度调节

Browse Source
This commit is contained in:
ywb
2026-05-29 08:51:01 +08:00
parent 3ccc49d6cf
commit a556a0858b
13 changed files with 701 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/mission_executor.py
+215 -132
View File
@@ -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)