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

可基本执行任务

Browse Source
This commit is contained in:
ywb
2026-05-28 13:55:55 +08:00
parent f507363c34
commit 4c096a4bd1
12 changed files with 1207 additions and 669 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/mission_executor.py
+279 -106
View File
@@ -23,6 +23,8 @@ import requests
import cv2
import numpy as np
from utils.nav2_navigator import Nav2Navigator, Nav2Status
logger = logging.getLogger(__name__)
ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
@@ -42,6 +44,8 @@ class MissionStatus(str, Enum):
PAUSED = "paused"
COMPLETED = "completed"
WAITING_QR = "waiting_qr"
WAITING_ERROR = "waiting_error"
WAITING_STEP = "waiting_step"
class MissionExecutorV3:
@@ -71,6 +75,22 @@ class MissionExecutorV3:
self._qr_event = threading.Event()
self._qr_value: Optional[str] = None
# 错误弹窗
self._error_choice = None # "skip" or "abort"
# 单步执行
self._single_step_mode = False
self._step_choice = None # "confirm", "retry", "abort"
self._error_mode = False # True when waiting for error resolution
# 错误弹窗
self._error_choice = None # "skip" or "abort"
# 单步执行
self._single_step_mode = False
self._step_choice = None # "confirm", "retry", "abort"
self._error_mode = False # True when waiting for error resolution
# 设备
from .arm_client import ArmClient
self.arm_client = ArmClient(
@@ -84,6 +104,9 @@ class MissionExecutorV3:
baudrate=config.get("baudrate", 1000000)
)
# Nav2 导航器（直接使用 rclpy BasicNavigator API，比 subprocess 更可靠）
self._nav = Nav2Navigator()
# ==================== 连接 ====================
def connect_all(self) -> Dict[str, bool]:
@@ -105,10 +128,23 @@ class MissionExecutorV3:
machines: list,
qr_configs: list,
models: list,
single_step: bool = False,
options: dict = None,
) -> dict:
"""
执行完整拍摄任务。
Args:
options: 任务步骤控制开关
- arm_init: 是否执行机械臂位置初始化
- agv_move: 是否执行AGV移动
- qr_scan: 是否执行二维码识别
- front_photo: 是否执行正面拍照
- back_photo: 是否执行背面拍照
"""
"""
执行完整拍摄任务。
Args:
mission_config: {rows, cols, grid, positions}
machines: [{id, row, col, front: {coords}, back: {coords}}]
@@ -131,8 +167,23 @@ class MissionExecutorV3:
grid = mission_config.get("grid", [])
positions = mission_config.get("positions", [])
# 如果 grid 为空，从 machines 重建
if not grid or all(not any(rw) if isinstance(rw, list) else True for rw in grid):
try:
cfg_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "data")
with open(os.path.join(cfg_dir, "machines_config.json")) as jf:
machines = json.load(jf)
grid = [[False] * cols for _ in range(rows)]
for m in machines:
r = int(m.get("row", 0))
c = int(m.get("col", 0))
if 0 <= r < rows and 0 <= c < cols:
grid[r][c] = True
except Exception:
grid = [[False] * cols for _ in range(rows)]
# 1. 生成蛇形路径
path = self._build_snake_path(rows, cols, grid)
path = MissionExecutorV3._build_snake_path(rows, cols, grid)
if not path:
self._log("❌ 网格中没有机器，任务终止")
self.report["error"] = "No machines in grid"
@@ -153,25 +204,43 @@ class MissionExecutorV3:
"photos_back": 0,
} for (r, c) in path]
# 初始化任务列表
self.report["tasks"] = [{
"row": r, "col": c,
"machine_id": f"m_{r}_{c}",
"label": f"{r+1}-{c+1}",
"status": "pending",
"step": "等待",
"qr_value": None,
"photos_front": 0,
"photos_back": 0,
} for (r, c) in path]
# 任务步骤控制开关
if options is None:
options = {}
opt_arm_init = options.get("arm_init", True)
opt_agv_move = options.get("agv_move", True)
# 机械臂初始化并入 AGV 移动：只要开移动，就先初始化机械臂
if opt_agv_move:
opt_arm_init = True
opt_qr_scan = options.get("qr_scan", True)
opt_front_photo = options.get("front_photo", True)
opt_back_photo = options.get("back_photo", True)
self._log(f"⚙️ 任务步骤: agv_move={opt_agv_move}, "
f"qr_scan={opt_qr_scan}, front={opt_front_photo}, back={opt_back_photo}")
# 机械臂初始姿态（AGV 移动前恢复）
arm_initial_pose = mission_config.get("arm_initial_pose", [0.0] * 6)
has_arm_pose = self.arm_client and any(abs(a) > 0.01 for a in arm_initial_pose)
# 2. 逐台执行
for idx, (r, c) in enumerate(path):
machine_idx = 0
while machine_idx < len(path):
if self._stop.is_set():
self._log("⏹️ 任务已停止")
break
self._wait_pause()
r, c = path[machine_idx]
rl, cl = r + 1, c + 1
# 恢复机械臂初始姿态
if opt_arm_init and has_arm_pose:
self._log(" 🦾 恢复机械臂初始姿态")
try:
self.arm_client.set_angles(arm_initial_pose, speed=500)
time.sleep(2)
except Exception as e:
self._log(f" ⚠️ 机械臂初始化失败: {e}")
# 更新任务状态 → 正面开始
task = self._get_task(r, c)
@@ -182,27 +251,36 @@ class MissionExecutorV3:
machine_id = f"m_{r}_{c}"
machine = next((m for m in machines if m.get("id") == machine_id), None)
if not machine:
self._log(f"⚠️ 机器 {r+1}-{c+1} 不存在，跳过")
self._log(f"⚠️ 机器 {rl}-{cl} 不存在，跳过")
machine_idx += 1
continue
rl, cl = r + 1, c + 1 # 显示用的 1-based
# --- 正面 ---
self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
self._step(f"机器 {rl}-{cl} 正面")
# 导航到正面点位
front_pt = self._find_point(positions, r, c, "front")
if front_pt and self._has_coords(front_pt):
if not self._navigate(front_pt, "正面"):
self._log(f"⚠️ 导航失败，尝试继续")
if opt_agv_move:
front_pt = self._find_point(positions, r, c, "front")
if front_pt and self._has_coords(front_pt):
if not self._navigate(front_pt, "正面"):
self._log(f"⚠️ 导航失败，尝试继续")
choice = self._wait_error(f"机器 {rl}-{cl} 正面导航失败")
if choice == "abort":
break
else:
self._log(f"⚠️ 无正面点位坐标")
else:
self._log(f"⚠️ 无正面点位坐标")
self._log(" ⏭️ 跳过AGV移动（正面）")
# 扫描二维码
qr_value = self._scan_qr_with_poses(qr_configs)
if self._stop.is_set():
break
qr_value = None
if opt_qr_scan:
qr_value = self._scan_qr_with_poses(qr_configs)
if self._stop.is_set():
break
else:
self._log(" ⏭️ 跳过二维码识别")
# 查机型 + 更新任务步骤
model_name = self._lookup_model(qr_value)
@@ -213,13 +291,16 @@ class MissionExecutorV3:
task["step"] = "正面拍照"
# 正面拍照
model = self._find_model(models, model_name)
if model:
self._shoot(model, "front", rl, cl, qr_value or "unknown")
if opt_front_photo:
model = self._find_model(models, model_name)
if model:
self._shoot(model, "front", rl, cl, qr_value or "unknown")
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
else:
self._log(f" ⚠️ 未找到机型 {model_name}")
self._log(" ⏭️ 跳过正面拍照")
self._progress(idx, 1)
self._progress(machine_idx, 1)
# --- 背面 ---
if task:
@@ -228,28 +309,53 @@ class MissionExecutorV3:
self._step(f"机器 {rl}-{cl} 背面")
# 导航到背面点位
back_pt = self._find_point(positions, r + 1, c, "back")
if back_pt and self._has_coords(back_pt):
if not self._navigate(back_pt, "背面"):
self._log(f"⚠️ 导航失败，尝试继续")
if opt_agv_move:
back_pt = self._find_point(positions, r + 1, c, "back")
if back_pt and self._has_coords(back_pt):
if not self._navigate(back_pt, "背面"):
self._log(f"⚠️ 导航失败，尝试继续")
choice = self._wait_error(f"机器 {rl}-{cl} 背面导航失败")
if choice == "abort":
break
else:
self._log(f"⚠️ 无背面点位坐标")
else:
self._log(f"⚠️ 无背面点位坐标")
self._log(" ⏭️ 跳过AGV移动（背面）")
# 背面拍照
if model:
self._shoot(model, "back", rl, cl, qr_value or "unknown")
if opt_back_photo:
if model:
self._shoot(model, "back", rl, cl, qr_value or "unknown")
else:
self._log(" ⏭️ 跳过背面拍照")
# 标记任务完成
if task:
task["status"] = "completed"
task["step"] = "完成"
self._progress(idx, 2)
self._progress(machine_idx, 2)
# 单步执行：等待用户确认
if single_step and not self._stop.is_set():
choice = self._wait_step_confirm(rl, cl)
if choice == "abort":
break
elif choice == "retry":
if task:
task["status"] = "pending"
task["step"] = "重试开始"
self._progress(machine_idx, 0)
continue # 不递增 machine_idx，重新执行
machine_idx += 1
# 3. 回到出发点
if not self._stop.is_set():
if not self._stop.is_set() and opt_agv_move:
self._step("返回出发点")
self._log("→ 返回 (0, 0)")
self._nav2_go_to_point(0, 0, 0, timeout_sec=60)
elif not self._stop.is_set():
self._log("⏭️ 跳过返回出发点")
elapsed = time.time() - start_time
return self._finish(elapsed)
@@ -275,17 +381,18 @@ class MissionExecutorV3:
# ==================== 蛇形路径 ====================
def _build_snake_path(self, rows: int, cols: int, grid: list) -> list:
@staticmethod
def _build_snake_path(rows: int, cols: int, grid: list) -> list:
"""奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左"""
path = []
for r in range(rows):
if r % 2 == 0:
for c in range(cols):
if self._has_machine(grid, r, c):
if MissionExecutorV3._has_machine(grid, r, c):
path.append((r, c))
else:
for c in range(cols - 1, -1, -1):
if self._has_machine(grid, r, c):
if MissionExecutorV3._has_machine(grid, r, c):
path.append((r, c))
return path
@@ -298,6 +405,52 @@ class MissionExecutorV3:
return c < len(row) and bool(row[c])
return False
@staticmethod
def _build_grid_from_machines(rows: int, cols: int, machines: list) -> list:
"""从机器列表重建 grid 矩阵"""
if not machines:
return [[False] * cols for _ in range(rows)]
max_r = max(int(m.get("row", 0)) for m in machines) + 1
max_c = max(int(m.get("col", 0)) for m in machines) + 1
gr = max(rows, max_r)
gc = max(cols, max_c)
grid = [[False] * gc for _ in range(gr)]
for m in machines:
r = int(m.get("row", 0))
c = int(m.get("col", 0))
if 0 <= r < gr and 0 <= c < gc:
grid[r][c] = True
return grid
@staticmethod
def pre_generate_tasks(mission_config: dict) -> list:
"""从网格配置预生成任务列表（用于 UI 展示，无需启动执行器）"""
rows = int(mission_config.get("rows", 1))
cols = int(mission_config.get("cols", 1))
grid = mission_config.get("grid", [])
# 如果 grid 为空但从 machines 重建
if not grid and machines:
grid = MissionExecutorV3._build_grid_from_machines(rows, cols, machines)
if grid:
rows = len(grid)
cols = len(grid[0]) if grid else cols
path = MissionExecutorV3._build_snake_path(rows, cols, grid)
tasks = []
for (r, c) in path:
tasks.append({
"row": r, "col": c,
"machine_id": f"m_{r}_{c}",
"label": f"{r+1}-{c+1}",
"status": "pending",
"step": "等待",
"qr_value": None,
"photos_front": 0,
"photos_back": 0,
})
return tasks
# ==================== 点位查找 ====================
@staticmethod
@@ -512,6 +665,7 @@ class MissionExecutorV3:
"step": self.report["step"],
"progress": self.report["progress"],
"total": self.report["total"],
"tasks": self.report.get("tasks", []),
}
def get_logs(self) -> dict:
@@ -545,6 +699,82 @@ class MissionExecutorV3:
99
)
# ==================== 错误弹窗 ====================
def _wait_error(self, msg: str) -> str:
"""阻塞等待用户选择：skip（跳过）或 abort（中断）"""
self.status = MissionStatus.WAITING_ERROR
self.report["status"] = "waiting_error"
self.report["step"] = msg
self.report["error"] = msg
self._log(f"⚠️ 错误处理: {msg}")
self._error_choice = None
self._error_mode = True
start = time.time()
while self._error_choice is None and not self._stop.is_set():
time.sleep(0.2)
if time.time() - start > 600: # 10分钟超时 → 跳过
self._error_choice = "skip"
choice = self._error_choice or "skip"
self._error_choice = None
self._error_mode = False
if choice == "abort":
self._stop.set()
self._log("⏹️ 用户选择中断")
else:
self._log("⏭️ 用户选择跳过")
# 恢复状态
self.status = MissionStatus.RUNNING if not self._single_step_mode else MissionStatus.WAITING_STEP
self.report["status"] = self.status.value
self.report["error"] = None
return choice
def set_error_choice(self, choice: str):
"""外部 API 设置错误处理选择"""
self._error_choice = choice
# ==================== 单步执行 ====================
def _wait_step_confirm(self, row_label: int, col_label: int) -> str:
"""单步执行：等待用户确认/重试/中断"""
self.status = MissionStatus.WAITING_STEP
self.report["status"] = "waiting_step"
self.report["step"] = f"机器 {row_label}-{col_label} 完成，等待确认"
self.report["current_step"] = {
"row": row_label - 1, "col": col_label - 1,
"label": f"{row_label}-{col_label}"
}
self._log(f"⏸️ 单步执行: 机器 {row_label}-{col_label} 完成，等待确认...")
self._step_choice = None
start = time.time()
while self._step_choice is None and not self._stop.is_set():
time.sleep(0.2)
if time.time() - start > 600: # 10分钟超时 → 确认
self._step_choice = "confirm"
choice = self._step_choice or "confirm"
self._step_choice = None
self.report.pop("current_step", None)
if choice == "abort":
self._stop.set()
self._log("⏹️ 用户选择中断")
elif choice == "retry":
self._log(f"🔄 用户选择重试机器 {row_label}-{col_label}")
else:
self._log(f"✅ 用户确认机器 {row_label}-{col_label}")
return choice
def set_step_choice(self, choice: str):
"""外部 API 设置单步执行选择"""
self._step_choice = choice
# ==================== Nav2 导航 ====================
# (保留原实现)
@@ -559,72 +789,15 @@ class MissionExecutorV3:
def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
timeout_sec: float = 120.0) -> bool:
if not self._nav2_check_available():
logger.error("Nav2 action server 不可用")
return False
qz = math.sin(yaw / 2.0)
qw = math.cos(yaw / 2.0)
pose_yaml = (
f"pose:\n"
f" header:\n"
f" stamp:\n"
f" sec: 0\n"
f" nanosec: 0\n"
f" frame_id: 'map'\n"
f" pose:\n"
f" position:\n"
f" x: {x}\n"
f" y: {y}\n"
f" z: 0.0\n"
f" orientation:\n"
f" x: 0.0\n"
f" y: 0.0\n"
f" z: {qz}\n"
f" w: {qw}"
)
cmd = (
f"ros2 action send_goal /navigate_to_pose "
f"nav2_msgs/action/NavigateToPose "
f"'{pose_yaml}' --feedback"
)
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
"""使用 Nav2Navigator 直接发送导航目标（blocking 模式，等待完成）"""
try:
process = subprocess.Popen(
full_cmd, shell=True,
stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
)
succeeded = False
elapsed = 0.0
while elapsed < timeout_sec:
import select
reads = [process.stdout]
ready, _, _ = select.select(reads, [], [], 1.0)
elapsed += 1.0
if process.stdout in ready:
line = process.stdout.readline()
if not line:
break
ls = line.strip()
if "succeeded" in ls.lower():
succeeded = True
break
elif "failed" in ls.lower() or "aborted" in ls.lower():
break
elif "canceled" in ls.lower() or "cancelled" in ls.lower():
break
if process.poll() is not None:
break
if process.poll() is None:
process.terminate()
try:
process.wait(timeout=3)
except subprocess.TimeoutExpired:
process.kill()
return succeeded
logger.info(f"🧭 导航到目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
ok = self._nav.navigate_to_pose(x, y, yaw, timeout_sec=timeout_sec, blocking=True)
if ok:
logger.info(f"✅ 导航成功到达 ({x:.3f}, {y:.3f})")
else:
logger.warning(f"⚠️ 导航失败或超时 ({x:.3f}, {y:.3f})")
return ok
except Exception as e:
logger.error(f"Nav2 异常: {e}")
return False