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 10:05:48 +08:00
parent 6ec778dc6d
commit f507363c34
3 changed files with 41 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/static/js/running.js
+1 -3
View File
@@ -21,8 +21,7 @@ createApp({
errorMsg: '',
waitingStep: false,
stepLabel: '',
// 任务步骤控制开关
armInitEnabled: true,
// 任务步骤控制开关（机械臂初始化并入AGV移动）
agvMoveEnabled: true,
qrScanEnabled: true,
frontPhotoEnabled: true,
@@ -121,7 +120,6 @@ createApp({
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({
arm_init: this.armInitEnabled,
agv_move: this.agvMoveEnabled,
qr_scan: this.qrScanEnabled,
front_photo: this.frontPhotoEnabled,
agv_app/templates/running.html
+5 -13
View File
@@ -4,7 +4,7 @@
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>运行监控 - AGV 拍摄系统</title>
<link rel="stylesheet" href="/static/css/style.css?v=20260527a">
<link rel="stylesheet" href="/static/css/style.css?v=20260527b">
</head>
<body>
<div id="app">
@@ -53,21 +53,13 @@
<h2>🎛️ 任务步骤控制</h2>
<p class="hint" style="margin-bottom:12px">关闭的步骤将在本次任务中跳过</p>
<div class="toggle-grid">
<div class="toggle-item">
<label class="toggle-switch">
<input type="checkbox" v-model="armInitEnabled">
<span class="toggle-slider"></span>
</label>
<span class="toggle-label">🦾 机械臂位置初始化</span>
<span class="toggle-hint">移动到每个点位前恢复初始姿态</span>
</div>
<div class="toggle-item">
<label class="toggle-switch">
<input type="checkbox" v-model="agvMoveEnabled">
<span class="toggle-slider"></span>
</label>
<span class="toggle-label">🚗 AGV移动</span>
<span class="toggle-hint">按之字形路线移动到各点位</span>
<span class="toggle-hint">含机械臂初始化，按之字形路线移动到各点位</span>
</div>
<div class="toggle-item">
<label class="toggle-switch">
@@ -122,8 +114,8 @@
</section>
<!-- 实时日志 -->
<section class="card" v-if="missionState === 'running' || missionState === 'waiting_qr'">
<h2>📜 实时日志</h2>
<section class="card">
<h2>📜 任务日志</h2>
<div class="log-box" ref="logBox">
<div v-for="(log, i) in logs" :key="i" class="log-line">[[ log ]]</div>
<div v-if="logs.length === 0" class="log-empty">等待任务开始...</div>
@@ -197,6 +189,6 @@
</div>
<script src="/static/js/vue3.global.prod.js"></script>
<script src="/static/js/running.js?v=20260527a"></script>
<script src="/static/js/running.js?v=20260527b"></script>
</body>
</html>
agv_app/utils/mission_executor.py
+35 -82
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"
@@ -102,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]:
@@ -204,10 +209,13 @@ class MissionExecutorV3:
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"⚙️ 任务步骤: arm_init={opt_arm_init}, agv_move={opt_agv_move}, "
self._log(f"⚙️ 任务步骤: agv_move={opt_agv_move}, "
f"qr_scan={opt_qr_scan}, front={opt_front_photo}, back={opt_back_photo}")
# 机械臂初始姿态（AGV 移动前恢复）
@@ -228,10 +236,11 @@ class MissionExecutorV3:
# 恢复机械臂初始姿态
if opt_arm_init and has_arm_pose:
self._log(" 🦾 恢复机械臂初始姿态")
self.arm_client.set_angles(arm_initial_pose, speed=500)
time.sleep(2)
elif not opt_arm_init:
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)
@@ -300,6 +309,14 @@ class MissionExecutorV3:
self._step(f"机器 {rl}-{cl} 背面")
# 导航到背面点位
# 导航到背面点位之前：恢复机械臂初始姿态
if opt_agv_move 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}")
if opt_agv_move:
back_pt = self._find_point(positions, r + 1, c, "back")
if back_pt and self._has_coords(back_pt):
@@ -468,37 +485,30 @@ class MissionExecutorV3:
# ==================== 二维码扫描 ====================
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)
# 扫码
time.sleep(2) # 等机械臂稳定 + 摄像头图像稳定
# 读取摄像头并扫码
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()
@@ -780,72 +790,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