From 7083c45feb1cbc21f1e08c146fda1e9c9a2e295a Mon Sep 17 00:00:00 2001 From: FaulknerWu Date: Fri, 19 Jun 2026 18:10:43 +0800 Subject: [PATCH] Update project structure --- .gitignore | 15 + agv_app/data/qr_config.json | 44 ++ agv_app/scripts/fix_scan_timestamp.py | 67 -- agv_app/scripts/start_agv.sh | 17 - agv_app/start.sh | 5 - agv_app/start_agv.sh | 17 - agv_app/start_flask.sh | 9 - agv_app/style.css | 1045 ------------------------- agv_app/utils/agv_controller.py | 161 ---- agv_app/utils/config.py | 92 --- agv_app/utils/map_navigator.py | 663 ---------------- arm/arm_server.py | 406 ++++++++++ arm_server/arm_server.service | 18 + docs/AGV_机械臂_技术说明文档.md | 887 +++++++++++++++++++++ docs/开发记录_2026-05_06.md | 151 ++++ restart_agv.sh | 31 - scripts/README.md | 72 ++ scripts/dev_start.sh | 39 + scripts/restart_flask.sh | 58 ++ {agv_app => scripts}/start_all.sh | 13 +- scripts/start_flask.sh | 20 + {agv_app => scripts}/stop_all.sh | 0 shared/config.json | 23 - start_agv.sh | 24 - 24 files changed, 1718 insertions(+), 2159 deletions(-) create mode 100644 .gitignore create mode 100644 agv_app/data/qr_config.json delete mode 100644 agv_app/scripts/fix_scan_timestamp.py delete mode 100644 agv_app/scripts/start_agv.sh delete mode 100644 agv_app/start.sh delete mode 100755 agv_app/start_agv.sh delete mode 100755 agv_app/start_flask.sh delete mode 100644 agv_app/style.css delete mode 100644 agv_app/utils/agv_controller.py delete mode 100644 agv_app/utils/config.py delete mode 100644 agv_app/utils/map_navigator.py create mode 100644 arm/arm_server.py create mode 100644 arm_server/arm_server.service create mode 100644 docs/AGV_机械臂_技术说明文档.md create mode 100644 docs/开发记录_2026-05_06.md delete mode 100644 restart_agv.sh create mode 100644 scripts/README.md create mode 100755 scripts/dev_start.sh create mode 100755 scripts/restart_flask.sh rename {agv_app => scripts}/start_all.sh (95%) create mode 100755 scripts/start_flask.sh rename {agv_app => scripts}/stop_all.sh (100%) delete mode 100644 shared/config.json delete mode 100644 start_agv.sh diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bf1157b --- /dev/null +++ b/.gitignore @@ -0,0 +1,15 @@ +__pycache__/ +*.py[cod] +.DS_Store +._* +.AppleDouble + +# Local runtime logs and temporary files +*.log +*.pid +timeout + +# Local backup copies +*.bak +*.bak.* +*.bak2 diff --git a/agv_app/data/qr_config.json b/agv_app/data/qr_config.json new file mode 100644 index 0000000..6dcc00c --- /dev/null +++ b/agv_app/data/qr_config.json @@ -0,0 +1,44 @@ +[ + { + "id": "qr_1779278140334", + "name": "二维码1", + "joint_angles": [ + -89.796645, + -2.013175, + -87.176721, + -82.49663, + -93.323403, + 20.399941 + ], + "qr_value": "BG042110276", + "model_id": "" + }, + { + "id": "qr_1779286233426", + "name": "左侧二维码", + "joint_angles": [ + -70.967019, + -19.319962, + -67.929797, + -90.749908, + -121.735483, + 20.399961 + ], + "qr_value": "", + "model_id": "" + }, + { + "id": "qr_1779954274845", + "name": "右侧二维码", + "joint_angles": [ + -106.216678, + 35.346758, + -134.01322, + -79.250251, + -84.069984, + 21.982971 + ], + "qr_value": "", + "model_id": "" + } +] \ No newline at end of file diff --git a/agv_app/scripts/fix_scan_timestamp.py b/agv_app/scripts/fix_scan_timestamp.py deleted file mode 100644 index 479190d..0000000 --- a/agv_app/scripts/fix_scan_timestamp.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python3 -"""修复激光雷达时间戳偏移的修正器 v5""" -import os, sys, rclpy -from rclpy.node import Node -from sensor_msgs.msg import LaserScan -from builtin_interfaces.msg import Time - -LOCKFILE = "/tmp/scan_fixer.lock" - -if os.path.exists(LOCKFILE): - with open(LOCKFILE) as f: - old_pid = int(f.read().strip()) - try: - os.kill(old_pid, 0) - print(f"Another fixer running PID {old_pid}, exit.", file=sys.stderr) - sys.exit(1) - except (OSError, ProcessLookupError): - print(f"Stale lock removed (PID {old_pid} dead)", file=sys.stderr) - -with open(LOCKFILE, "w") as f: - f.write(str(os.getpid())) - -def main(): - rclpy.init(args=sys.argv[1:]) - node = Node('scan_timestamp_fixer') - offset = 2.0 - pub = node.create_publisher(LaserScan, '/scan_corrected', 10) - count = [0] - - def cb(msg: LaserScan): - count[0] += 1 - s, ns = msg.header.stamp.sec, msg.header.stamp.nanosec - s2 = s - int(offset) - ns2 = ns - int((offset % 1) * 1e9) - if ns2 < 0: - ns2 += 1000000000 - s2 -= 1 - out = LaserScan() - out.header.frame_id = msg.header.frame_id - out.header.stamp = Time(sec=s2, nanosec=ns2) - out.angle_min = msg.angle_min - out.angle_max = msg.angle_max - out.angle_increment = msg.angle_increment - out.time_increment = msg.time_increment - out.scan_time = msg.scan_time - out.range_min = msg.range_min - out.range_max = msg.range_max - out.ranges = msg.ranges - out.intensities = msg.intensities - pub.publish(out) - if count[0] % 200 == 0: - node.get_logger().info(f'#{count[0]} /scan={s} -> /scan_corrected={s2}') - - node.create_subscription(LaserScan, '/scan', cb, 10) - node.get_logger().info(f'Fixer PID={os.getpid()}, offset={offset}s') - - try: - while rclpy.ok(): - rclpy.spin_once(node, timeout_sec=0.5) - finally: - node.destroy_node() - rclpy.shutdown() - if os.path.exists(LOCKFILE): - os.unlink(LOCKFILE) - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/agv_app/scripts/start_agv.sh b/agv_app/scripts/start_agv.sh deleted file mode 100644 index f2c548a..0000000 --- a/agv_app/scripts/start_agv.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/bash -source /opt/ros/humble/setup.bash -source /home/elephant/agv_pro_ros2/install/setup.bash -export ROS_DOMAIN_ID=1 -cd /home/elephant/agv_pro_ros2 -nohup ros2 daemon start >/dev/null 2>&1 & -sleep 5 -nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 & -sleep 8 -nohup python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py > /tmp/scan_fixer.log 2>&1 & -sleep 5 -nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py autostart:=True > /tmp/ros2_nav2.log 2>&1 & -sleep 15 -cd /home/elephant/work/agv_app && nohup python3 app.py > /tmp/agv_flask.log 2>&1 & -sleep 5 -echo "ALL_STARTED" -ps aux | grep -E 'lslidar|agv_pro_node|nav2_container|scan_timestamp_fixer|ros2-daemon|app.py' | grep -v grep \ No newline at end of file diff --git a/agv_app/start.sh b/agv_app/start.sh deleted file mode 100644 index ad9d646..0000000 --- a/agv_app/start.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash -# 启动 AGV 拍摄系统 - -cd ~/work/agv_app -python3 app.py diff --git a/agv_app/start_agv.sh b/agv_app/start_agv.sh deleted file mode 100755 index f2c548a..0000000 --- a/agv_app/start_agv.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/bash -source /opt/ros/humble/setup.bash -source /home/elephant/agv_pro_ros2/install/setup.bash -export ROS_DOMAIN_ID=1 -cd /home/elephant/agv_pro_ros2 -nohup ros2 daemon start >/dev/null 2>&1 & -sleep 5 -nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 & -sleep 8 -nohup python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py > /tmp/scan_fixer.log 2>&1 & -sleep 5 -nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py autostart:=True > /tmp/ros2_nav2.log 2>&1 & -sleep 15 -cd /home/elephant/work/agv_app && nohup python3 app.py > /tmp/agv_flask.log 2>&1 & -sleep 5 -echo "ALL_STARTED" -ps aux | grep -E 'lslidar|agv_pro_node|nav2_container|scan_timestamp_fixer|ros2-daemon|app.py' | grep -v grep \ No newline at end of file diff --git a/agv_app/start_flask.sh b/agv_app/start_flask.sh deleted file mode 100755 index 8e50f0b..0000000 --- a/agv_app/start_flask.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/bin/bash -# Flask 启动脚本 - 杀掉旧进程并重启 - -pkill -f "python.*app.py" 2>/dev/null -sleep 1 - -cd /home/elephant/work/agv_app -nohup python3 app.py > /tmp/agv_flask.log 2>&1 & -echo "Flask started, PID: $!" diff --git a/agv_app/style.css b/agv_app/style.css deleted file mode 100644 index a28ea53..0000000 --- a/agv_app/style.css +++ /dev/null @@ -1,1045 +0,0 @@ -/* ========== 全局样式 ========== */ -* { box-sizing: border-box; margin: 0; padding: 0; } - -body { - font-family: -apple-system, "PingFang SC", "Microsoft YaHei", sans-serif; - background: #0f1923; - color: #e8eaed; - font-size: 14px; - min-height: 100vh; -} - -a { color: #4fc3f7; text-decoration: none; } -a:hover { text-decoration: underline; } - -.container { - max-width: 1200px; - margin: 0 auto; - padding: 20px 16px; - display: grid; - grid-template-columns: 1fr 1fr; - gap: 16px; -} - -/* ========== 顶部栏 ========== */ -.topbar { - background: #1a2332; - border-bottom: 1px solid #2a3441; - padding: 0 20px; - display: flex; - align-items: center; - height: 56px; - gap: 32px; - position: sticky; - top: 0; - z-index: 100; -} - -.logo { font-size: 18px; font-weight: bold; color: #4fc3f7; } - -.nav { display: flex; gap: 4px; } -.nav-link { - padding: 8px 16px; - border-radius: 6px; - color: #9aa0a6; - transition: all 0.2s; -} -.nav-link:hover { background: #2a3441; color: #e8eaed; text-decoration: none; } -.nav-link.active { background: #263238; color: #4fc3f7; } - -.status-bar { margin-left: auto; display: flex; gap: 12px; } -.status-item { - padding: 4px 12px; - border-radius: 12px; - font-size: 12px; - font-weight: bold; -} -.status-item.setting { background: #1b3a2f; color: #4caf50; } -.status-item.running { background: #2a2a1b; color: #ffeb3b; } -.status-item.paused { background: #3a2a1a; color: #ff9800; } -.status-item.idle { background: #2a2a2a; color: #9aa0a6; } - -/* ========== 卡片 ========== */ -.card { - background: #1a2332; - border-radius: 12px; - padding: 20px; - border: 1px solid #2a3441; -} -.card h2 { font-size: 16px; margin-bottom: 16px; color: #4fc3f7; } - -/* ========== 状态卡片 ========== */ -.grid-3 { display: grid; grid-template-columns: repeat(3, 1fr); gap: 12px; margin-bottom: 16px; } -.status-card { - background: #0f1923; - border-radius: 8px; - padding: 16px; - text-align: center; - border: 1px solid #2a3441; -} -.status-card.ok { border-color: #2e7d32; background: #0d1f14; } -.status-card.error { border-color: #c62828; background: #1f0d0d; } -.status-icon { font-size: 24px; margin-bottom: 8px; } -.status-label { font-size: 12px; color: #9aa0a6; margin-bottom: 4px; } -.status-value { font-size: 14px; font-weight: bold; } - -/* ========== 按钮 ========== */ -.btn { - padding: 8px 16px; - border-radius: 6px; - border: none; - cursor: pointer; - font-size: 14px; - transition: all 0.2s; - background: #263238; - color: #e8eaed; - font-family: inherit; -} -.btn:hover:not(:disabled) { background: #37474f; } -.btn:disabled { opacity: 0.5; cursor: not-allowed; } - -.btn-primary { background: #0277bd; color: #fff; } -.btn-primary:hover:not(:disabled) { background: #0288d1; } -.btn-secondary { background: #37474f; } -.btn-danger { background: #d32f2f; color: #fff; } -.btn-danger:hover:not(:disabled) { background: #f44336; } -.btn-success { background: #2e7d32; color: #fff; } -.btn-success:hover:not(:disabled) { background: #388e3c; } -.btn-warning { background: #e65100; color: #fff; } -.btn-error { background: #c62828; color: #fff; } -.btn-large { padding: 12px 24px; font-size: 16px; } -.btn-small { padding: 4px 10px; font-size: 12px; } -.btn-icon { background: none; border: none; cursor: pointer; font-size: 14px; padding: 4px; } -.btn-row { display: flex; gap: 8px; margin-top: 12px; flex-wrap: wrap; } - -/* ========== 表单 ========== */ -.form-group { margin-bottom: 12px; } -.form-group label { display: block; font-size: 12px; color: #9aa0a6; margin-bottom: 4px; } -.form-group input, -.form-group select { - width: 100%; - padding: 8px 12px; - background: #0f1923; - border: 1px solid #2a3441; - border-radius: 6px; - color: #e8eaed; - font-size: 14px; - font-family: inherit; -} -.form-group input:focus, -.form-group select:focus { outline: none; border-color: #4fc3f7; } -.form-row { display: grid; grid-template-columns: 1fr 1fr; gap: 12px; } - -/* ========== Tabs ========== */ -.tabs { - background: #1a2332; - border-bottom: 1px solid #2a3441; - padding: 0 20px; - display: flex; - gap: 4px; -} -.tab { - padding: 12px 20px; - background: none; - border: none; - color: #9aa0a6; - cursor: pointer; - font-size: 14px; - border-bottom: 2px solid transparent; - font-family: inherit; -} -.tab.active { color: #4fc3f7; border-bottom-color: #4fc3f7; } -.tab:hover { color: #e8eaed; } - -/* ========== 摄像头预览 ========== */ -.camera-preview { - width: 100%; - max-width: 480px; - border-radius: 8px; - overflow: hidden; - margin: 0 auto 16px; - background: #000; -} -.camera-preview img, -.camera-full img { - width: 100%; - display: block; - aspect-ratio: 16/9; - object-fit: cover; -} -.camera-full { - width: 100%; - border-radius: 8px; - overflow: hidden; - background: #000; -} - -/* ========== 关节控制 ========== */ -.joints-panel { margin-top: 16px; } -.joints-panel h3 { margin-bottom: 12px; font-size: 14px; } -.joint-grid { display: grid; grid-template-columns: repeat(3, 1fr); gap: 12px; } -.joint-control { - background: #0f1923; - border-radius: 8px; - padding: 12px; - text-align: center; - border: 1px solid #2a3441; -} -.joint-control label { font-size: 12px; color: #4fc3f7; font-weight: bold; } -.joint-value { font-size: 18px; font-weight: bold; color: #fff; margin: 4px 0; } -.joint-buttons { display: flex; align-items: center; gap: 4px; justify-content: center; } -.joint-buttons button { - width: 32px; - height: 32px; - border-radius: 4px; - border: 1px solid #2a3441; - background: #263238; - color: #e8eaed; - cursor: pointer; - font-size: 14px; -} -.joint-buttons input { - width: 60px; - padding: 4px; - text-align: center; - background: #0f1923; - border: 1px solid #2a3441; - border-radius: 4px; - color: #e8eaed; - font-size: 12px; -} - -/* ========== 点位列表 ========== */ -.point-item { - background: #0f1923; - border: 1px solid #2a3441; - border-radius: 8px; - padding: 12px; - margin-bottom: 12px; -} -.point-header { display: flex; align-items: center; gap: 8px; margin-bottom: 8px; } -.point-name { font-weight: bold; font-size: 15px; } -.point-coords { font-size: 12px; color: #9aa0a6; margin-bottom: 8px; } -.badge { - padding: 2px 8px; - border-radius: 10px; - font-size: 11px; - background: #263238; - color: #4fc3f7; -} -.pose-list { margin-top: 8px; } -.pose-item { - display: flex; - align-items: center; - gap: 8px; - padding: 6px 0; - border-bottom: 1px solid #2a3441; - font-size: 13px; -} -.angles { color: #9aa0a6; font-size: 11px; font-family: monospace; } -.pose-add { - display: flex; - gap: 8px; - align-items: center; - margin-top: 8px; -} -.pose-add input { flex: 1; padding: 6px 10px; background: #1a2332; border: 1px solid #2a3441; border-radius: 4px; color: #e8eaed; font-size: 13px; } -.pose-add select { padding: 6px; background: #1a2332; border: 1px solid #2a3441; border-radius: 4px; color: #e8eaed; } - -.empty-hint { color: #9aa0a6; text-align: center; padding: 20px; } -.hint { font-size: 12px; color: #9aa0a6; margin-top: 8px; } -.alert { padding: 12px 16px; border-radius: 8px; margin-bottom: 12px; } -.alert-error { background: #1f0d0d; border: 1px solid #c62828; color: #ef5350; } -.checkbox-group { display: flex; gap: 16px; } -.checkbox-group label { display: flex; align-items: center; gap: 6px; cursor: pointer; color: #e8eaed; } - -/* ========== 运行页面 ========== */ -.running-header { display: flex; align-items: center; gap: 20px; margin-bottom: 16px; } -.running-status { - font-size: 18px; - font-weight: bold; - display: flex; - align-items: center; - gap: 8px; -} -.running-status.idle { color: #9aa0a6; } -.running-status.running { color: #4caf50; } -.running-status.paused { color: #ff9800; } -.pulse { - width: 10px; - height: 10px; - border-radius: 50%; - background: currentColor; - animation: pulse 1.5s infinite; -} -@keyframes pulse { - 0%, 100% { opacity: 1; transform: scale(1); } - 50% { opacity: 0.5; transform: scale(0.8); } -} -.running-progress { flex: 1; display: flex; align-items: center; gap: 12px; } -.progress-bar { flex: 1; height: 8px; background: #2a3441; border-radius: 4px; overflow: hidden; } -.progress-fill { height: 100%; background: #4fc3f7; border-radius: 4px; transition: width 0.3s; } - -/* ========== 报告 ========== */ -.report-summary { display: flex; gap: 16px; margin-bottom: 16px; } -.stat { padding: 8px 16px; border-radius: 8px; background: #0f1923; border: 1px solid #2a3441; } -.stat.ok { border-color: #2e7d32; color: #4caf50; } -.stat.error { border-color: #c62828; color: #ef5350; } -.report-item { padding: 8px 12px; background: #0f1923; border-radius: 6px; margin-bottom: 8px; border: 1px solid #2a3441; } -.report-point { display: flex; align-items: center; gap: 8px; font-weight: bold; } -.report-status { font-size: 16px; } -.report-pose { font-size: 12px; color: #9aa0a6; padding-left: 24px; margin-top: 4px; } - -/* ========== 响应式 ========== */ -@media (max-width: 768px) { - .container { grid-template-columns: 1fr; } - .grid-3 { grid-template-columns: 1fr; } - .form-row { grid-template-columns: 1fr; } - .joint-grid { grid-template-columns: repeat(2, 1fr); } - .form-row { grid-template-columns: 1fr; } -} - -/* AGV 移动控制面板 */ -.agv-status-bar { - display: flex; - gap: 16px; - align-items: center; - padding: 10px 14px; - background: #0f1923; - border-radius: 8px; - margin-bottom: 16px; - font-size: 13px; - color: #9aa0a6; - flex-wrap: wrap; -} -.agv-status-bar strong { color: #e8eaed; } - -.agv-control-panel { - display: flex; - flex-direction: column; - align-items: center; - gap: 6px; - max-width: 280px; - margin: 0 auto; -} -.agv-dir-row { - display: grid; - grid-template-columns: 80px 80px 80px; - gap: 6px; - width: 100%; -} -.agv-dir-placeholder { width: 80px; height: 44px; } -.agv-btn { - height: 44px; - border-radius: 8px; - border: 1px solid #2a3441; - background: #263238; - color: #e8eaed; - cursor: pointer; - font-size: 13px; - font-family: inherit; - transition: background 0.15s; - display: flex; - align-items: center; - justify-content: center; - gap: 4px; - user-select: none; -} -.agv-btn:active, .agv-btn:focus { outline: none; } -.agv-btn-up { background: #1b3a2f; border-color: #2e7d32; color: #4caf50; } -.agv-btn-down { background: #3a1b1b; border-color: #7d2e2e; color: #f44336; } -.agv-btn-left { background: #1b2d3a; border-color: #1565c0; color: #42a5f5; } -.agv-btn-right { background: #2d2a1b; border-color: #7d6e2e; color: #ffc107; } -.agv-btn-stop { background: #37474f; border-color: #546e7a; } -.agv-btn-up:active { background: #1e4d38; } -.agv-btn-down:active { background: #4d2020; } -.agv-btn-left:active { background: #1e3a4d; } -.agv-btn-right:active { background: #3d3820; } -.agv-btn-stop:active { background: #455a64; } -.agv-btn-lateral { - background: #2d1b4a; - border-color: #7c4dff; - color: #b388ff; - font-size: 13px; - min-width: 120px; -} -.agv-btn-lateral:active { background: #3d2560; } -.agv-lateral-row { - display: flex; - gap: 12px; - justify-content: center; - margin-top: 8px; - max-width: 280px; - width: 100%; -} - -.speed-control { - display: flex; - align-items: center; - gap: 10px; -} -.speed-value { - min-width: 44px; - text-align: right; - font-weight: bold; - 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; - grid-template-columns: 1fr 1fr; - gap: 16px; - margin-top: 12px; -} -.camera-box { - background: #111; - border-radius: 8px; - overflow: hidden; -} -.camera-label { - padding: 8px 12px; - font-size: 13px; - color: #aaa; - background: #1a1a1a; - border-bottom: 1px solid #333; -} -.camera-img { - width: 100%; - display: block; - aspect-ratio: 4/3; - object-fit: cover; -} -.camera-placeholder { - width: 100%; - aspect-ratio: 4/3; - display: flex; - align-items: center; - justify-content: center; - color: #666; - font-size: 14px; -} -/* ========== 地图标记 ========== */ -.map-marker { - position: absolute; - transform: translate(-50%, -100%); - font-size: 20px; - cursor: pointer; - filter: drop-shadow(0 2px 4px rgba(0,0,0,0.5)); - z-index: 10; -} -.map-marker:hover { - transform: translate(-50%, -100%) scale(1.2); -} - -/* ========== 任务配置 M×N 网格 ========== */ -.mission-grid-wrap { - margin-top: 12px; - overflow-x: auto; -} -.mission-grid { - display: grid; - gap: 4px; - grid-template-columns: 80px repeat(var(--cols,4), 90px); -} -.grid-cell { - min-width: 80px; - min-height: 48px; - display: flex; - align-items: center; - justify-content: center; - border-radius: 6px; - font-size: 12px; - cursor: pointer; - border: 1px solid #2a3441; - background: #0f1923; - transition: background 0.15s, border-color 0.15s; -} -.grid-cell.active { - background: #1b3a2f; - border-color: #2e7d32; - color: #4caf50; -} -.grid-cell.active:hover { - background: #234; -} -.grid-cell.selected { - border-color: #4fc3f7 !important; - box-shadow: 0 0 0 2px #4fc3f7; -} -.grid-header { - background: transparent; - border-color: transparent; - cursor: default; - font-weight: bold; - color: #9aa0a6; - font-size: 12px; -} - -/* 机器配置表单 */ -.machine-form { - background: #0f1923; - border: 1px solid #2a3441; - border-radius: 8px; - padding: 16px; - margin-top: 12px; -} -.machine-form h3 { - font-size: 14px; - color: #4fc3f7; - margin-bottom: 10px; -} -.machine-form h4 { - font-size: 13px; - color: #9aa0a6; - margin: 8px 0 6px; -} - -/* 姿态列表 */ -.pose-list { - margin-top: 8px; -} -.pose-item { - display: flex; - align-items: center; - gap: 8px; - padding: 6px 0; - border-bottom: 1px solid #2a3441; - font-size: 13px; -} -.pose-name { - font-weight: bold; - min-width: 80px; -} -.pose-angles { - color: #9aa0a6; - font-size: 11px; - font-family: monospace; - flex: 1; -} -.pose-add { - display: flex; - gap: 8px; - align-items: center; - margin-top: 8px; -} -.pose-add input { - flex: 1; - padding: 6px 10px; - background: #1a2332; - border: 1px solid #2a3441; - border-radius: 4px; - color: #e8eaed; - font-size: 13px; -} - -/* 蛇形序列预览 */ -.sequence-preview { - display: flex; - flex-direction: column; - gap: 4px; - max-height: 320px; - overflow-y: auto; -} -.sequence-step { - display: flex; - align-items: center; - gap: 10px; - padding: 6px 12px; - background: #0f1923; - border-radius: 6px; - border: 1px solid #2a3441; - font-size: 13px; -} -.step-index { - background: #263238; - color: #4fc3f7; - border-radius: 10px; - min-width: 28px; - height: 22px; - display: flex; - align-items: center; - justify-content: center; - font-size: 11px; - font-weight: bold; -} -.step-info { - flex: 1; -} -.step-side { - padding: 2px 8px; - border-radius: 8px; - font-size: 11px; - font-weight: bold; -} -.step-side:contains('正面') { - background: #1b3a2f; - color: #4caf50; -} -.step-side:contains('背面') { - background: #3a1b2f; - color: #ce93d8; -} - -/* 网格单元格点位配置 */ -.cell-machine { - font-size: 11px; - font-weight: bold; - color: #2c3e50; -} -.cell-points { - margin-top: 2px; - font-size: 9px; -} -.point-row { - display: flex; - align-items: center; - gap: 2px; - padding: 1px 2px; - background: #f8f9fa; - border-radius: 3px; - cursor: pointer; - margin: 1px 0; -} -.point-row:hover { - background: #e9ecef; -} -.point-label { - color: #666; - min-width: 24px; -} -.point-coords { - color: #0366d6; - font-family: monospace; - font-size: 8px; - flex: 1; -} -.btn-icon-small { - background: none; - border: none; - cursor: pointer; - font-size: 10px; - padding: 1px 3px; - border-radius: 3px; -} -.btn-icon-small:hover { - background: #ddd; -} - -/* ========== 任务配置 弹窗 + 网格增强样式 ========== */ - -/* 弹窗遮罩 */ -.modal-overlay { - position: fixed; - inset: 0; - background: rgba(0,0,0,0.65); - display: flex; - align-items: center; - justify-content: center; - z-index: 1000; -} -.modal-box { - background: #1a1f2e; - border: 1px solid #2a3a50; - border-radius: 12px; - padding: 20px 24px; - min-width: 380px; - max-width: 500px; - box-shadow: 0 8px 32px rgba(0,0,0,0.5); -} -.modal-box h3 { margin: 0 0 8px; color: #e0e6f0; font-size: 16px; } - -/* 点位行单元格 */ -.point-cell { cursor: pointer; flex-direction: column; gap: 2px; } -.point-cell:hover { border-color: #4fc3f7; background: #162030; } -.point-cell.point-filled { background: #0d2535; border-color: #1565c0; } -.point-coords { font-size: 10px; color: #64b5f6; font-family: monospace; } -.point-empty { font-size: 10px; color: #455a64; } - -/* 机器行单元格 */ -.machine-cell { cursor: pointer; } -.machine-cell:hover { border-color: #4caf50; background: #1b3a2f; } -.machine-cell.active { background: #1b3a2f; border-color: #2e7d32; color: #4caf50; } -.machine-icon { font-size: 18px; } -.machine-empty { font-size: 16px; color: #455a64; } -/* ========== 任务配置 弹窗 + 网格增强样式 ========== */ - -/* 弹窗遮罩 */ -.modal-overlay { - position: fixed; - inset: 0; - background: rgba(0,0,0,0.65); - display: flex; - align-items: center; - justify-content: center; - z-index: 1000; -} -.modal-box { - background: #1a1f2e; - border: 1px solid #2a3a50; - border-radius: 12px; - padding: 20px 24px; - min-width: 380px; - max-width: 500px; - box-shadow: 0 8px 32px rgba(0,0,0,0.5); -} -.modal-box h3 { margin: 0 0 8px; color: #e0e6f0; font-size: 16px; } - -/* 点位行单元格 */ -.point-cell { cursor: pointer; flex-direction: column; gap: 2px; } -.point-cell:hover { border-color: #4fc3f7; background: #162030; } -.point-cell.point-filled { background: #0d2535; border-color: #1565c0; } -.point-coords { font-size: 10px; color: #64b5f6; font-family: monospace; } -.point-empty { font-size: 10px; color: #455a64; } - -/* 机器行单元格 */ -.machine-cell { cursor: pointer; } -.machine-cell:hover { border-color: #4caf50; background: #1b3a2f; } -.machine-cell.active { background: #1b3a2f; border-color: #2e7d32; color: #4caf50; } -.machine-icon { font-size: 18px; } -.machine-empty { font-size: 16px; color: #455a64; } -/* 点位编辑弹窗 */ -.modal-overlay .modal-box { min-width: 420px; } -.modal-overlay .form-row { gap: 8px; } -.modal-overlay .btn-row { gap: 8px; flex-wrap: wrap; } - -/* 地图坐标点覆盖层 */ -.map-container { position: relative; } -.map-overlay { - position: absolute; top: 0; left: 0; right: 0; bottom: 0; - pointer-events: none; z-index: 10; -} -.map-dot { - position: absolute; - transform: translate(-50%, -50%); -} -.point-dot { - width: 10px; height: 10px; - background: #f39c12; - border-radius: 50%; - border: 2px solid #fff; - box-shadow: 0 0 6px rgba(243,156,18,0.9); -} -/* AGV 实时位置点 */ -.agv-dot { - width: 14px !important; - height: 14px !important; - background: #ff5722 !important; - border: 2px solid #fff !important; - border-radius: 50% !important; - box-shadow: 0 0 8px #ff5722, 0 0 16px #ff572288 !important; - animation: agv-pulse 1.5s ease-in-out infinite !important; - z-index: 10 !important; -} -@keyframes agv-pulse { - 0%, 100% { transform: scale(1); opacity: 1; } - 50% { transform: scale(1.3); opacity: 0.7; } -} - -/* 二维码位置点 */ -.qr-dot { - width: 12px !important; - height: 12px !important; - background: #ff9800 !important; - border: 2px solid #fff !important; - border-radius: 3px !important; - box-shadow: 0 0 8px #ff9800, 0 0 14px #ff980088 !important; - animation: qr-pulse 2s ease-in-out infinite !important; - z-index: 10 !important; -} -@keyframes qr-pulse { - 0%, 100% { transform: scale(1); opacity: 1; } - 50% { transform: scale(1.2); opacity: 0.6; } -} - -/* 机器行：有机/无机器 切换 */ -.machine-toggle { - display: flex; - align-items: center; - gap: 6px; - cursor: pointer; - user-select: none; -} -.machine-toggle input[type="checkbox"] { - accent-color: #4caf50; - width: 16px; - height: 16px; - cursor: pointer; -} -.machine-status.on { - color: #4caf50; - font-size: 12px; -} -.machine-status.off { - color: #666; - font-size: 12px; -} - -/* ========== 实时日志 ========== */ -.log-box { - background: #0a0a0a; - color: #00ff88; - font-family: 'Courier New', 'Menlo', monospace; - font-size: 13px; - line-height: 1.6; - max-height: 320px; - overflow-y: auto; - padding: 12px 16px; - border-radius: 6px; - margin-top: 8px; - border: 1px solid #1a1a1a; -} -.log-line { - padding: 2px 0; - border-bottom: 1px solid #111; - word-break: break-all; -} -.log-empty { - color: #555; - font-style: italic; - padding: 12px 0; -} - -/* ========== 弹窗 ========== */ -.modal-overlay { - position: fixed; - top: 0; - left: 0; - width: 100%; - height: 100%; - background: rgba(0, 0, 0, 0.75); - display: flex; - align-items: center; - justify-content: center; - z-index: 9999; -} -.modal { - background: #1a1a2e; - padding: 28px 32px; - border-radius: 12px; - min-width: 400px; - max-width: 90%; - box-shadow: 0 8px 32px rgba(0, 0, 0, 0.5); -} -.modal h3 { - margin: 0 0 12px 0; - color: #e0e0e0; -} -.modal p { - color: #aaa; - margin: 0 0 16px 0; -} -.modal input[type="text"] { - width: 100%; - padding: 10px 12px; - background: #0a0a0a; - border: 1px solid #333; - border-radius: 6px; - color: #e0e0e0; - font-size: 15px; - outline: none; - box-sizing: border-box; -} -.modal input[type="text"]:focus { - border-color: #409eff; -} -.modal-actions { - display: flex; - gap: 12px; - margin-top: 20px; -} -.modal-actions .btn { - flex: 1; -} - -/* ========== 任务清单 ========== */ -.task-grid { - display: grid; - grid-template-columns: repeat(auto-fill, minmax(130px, 1fr)); - gap: 10px; - margin-top: 10px; -} -.task-cell { - background: #0a0a0a; - border: 1px solid #1a1a1a; - border-radius: 8px; - padding: 12px; - text-align: center; - transition: all 0.3s; -} -.task-cell.task-active { - border-color: #409eff; - background: #0d1b2a; -} -.task-cell.task-completed { - border-color: #4caf50; - opacity: 0.7; -} -.task-cell.task-active .task-step-text { - color: #409eff; - font-weight: bold; -} -.task-pos { - font-size: 16px; - font-weight: bold; - color: #e0e0e0; - margin-bottom: 6px; -} -.task-status-icon { - font-size: 20px; - margin-bottom: 4px; -} -.task-step-text { - font-size: 12px; - color: #888; - margin-bottom: 4px; -} -.task-info { - font-size: 11px; - color: #666; -} -.task-qr { - font-family: monospace; - color: #aaa; -} -.task-photos { - color: #888; -} -.pulse-icon { - animation: taskPulse 1s infinite; -} -@keyframes taskPulse { - 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/utils/agv_controller.py b/agv_app/utils/agv_controller.py deleted file mode 100644 index 8ce8d23..0000000 --- a/agv_app/utils/agv_controller.py +++ /dev/null @@ -1,161 +0,0 @@ -""" -AGV 导航控制模块 - 通过 pymycobot 控制 AGV 运动 -""" -import time -import logging -from typing import Tuple, Optional, List - -logger = logging.getLogger(__name__) - -# 尝试导入 pymycobot -try: - from pymycobot import MyAGVPro - MYCOBOT_AVAILABLE = True -except ImportError: - MYCOBOT_AVAILABLE = False - logger.warning("pymycobot 未安装，AGV 控制功能不可用") - - -class AGVController: - """AGV 运动控制""" - - def __init__(self, device: str = "/dev/agvpro_controller", baudrate: int = 1000000): - self.device = device - self.baudrate = baudrate - self._agv: Optional[MyAGVPro] = None - self._connected = False - - def connect(self) -> bool: - """连接 AGV""" - if not MYCOBOT_AVAILABLE: - logger.error("pymycobot 不可用") - return False - try: - self._agv = MyAGVPro(self.device, self.baudrate, debug=False) - # 检查是否上电 - if self._agv.is_power_on(): - self._connected = True - logger.info("AGV 连接成功") - return True - else: - logger.warning("AGV 未上电，尝试上电...") - self._agv.power_on() - time.sleep(2) - if self._agv.is_power_on(): - self._connected = True - return True - return False - except Exception as e: - logger.error(f"AGV 连接失败: {e}") - return False - - def is_connected(self) -> bool: - return self._connected and self._agv is not None - - def move_forward(self, speed: float = 0.5, duration: float = None): - """前进""" - if not self.is_connected(): - return - self._agv.move_forward(speed) - if duration: - time.sleep(duration) - self.stop() - - def move_backward(self, speed: float = 0.5, duration: float = None): - """后退""" - if not self.is_connected(): - return - self._agv.move_backward(speed) - if duration: - time.sleep(duration) - self.stop() - - def turn_left(self, speed: float = 0.5, duration: float = None): - """左转""" - if not self.is_connected(): - return - self._agv.turn_left(speed) - if duration: - time.sleep(duration) - self.stop() - - def turn_right(self, speed: float = 0.5, duration: float = None): - """右转""" - if not self.is_connected(): - return - self._agv.turn_right(speed) - if duration: - time.sleep(duration) - self.stop() - - def move_left_lateral(self, speed: float = 0.5, duration: float = None): - """向左横向移动""" - if not self.is_connected(): - return - self._agv.move_left_lateral(speed) - if duration: - time.sleep(duration) - self.stop() - - def move_right_lateral(self, speed: float = 0.5, duration: float = None): - """向右横向移动""" - if not self.is_connected(): - return - self._agv.move_right_lateral(speed) - if duration: - time.sleep(duration) - self.stop() - - def stop(self): - """停止""" - if self.is_connected(): - self._agv.stop() - - def get_position(self) -> Optional[List[float]]: - """获取 AGV 当前位置 [x, y, rz]""" - if not self.is_connected(): - return None - try: - # 启用自动报告以获取位置 - self._agv.set_auto_report_state(1) - time.sleep(0.5) - msg = self._agv.get_auto_report_message() - if msg and len(msg) >= 3: - return [msg[0], msg[1], msg[2]] - except Exception as e: - logger.error(f"获取 AGV 位置失败: {e}") - return None - - def go_to_point(self, x: float, y: float, rz: float = None, speed: float = 0.5) -> bool: - """移动到目标点（简单的方向控制实现）""" - # 注意：AGV Pro 的 pymycobot 没有直接 goto API - # 需要 ROS2 SLAM 导航支持，此处提供基础运动接口 - # 实际导航需要结合地图和路径规划 - logger.warning("go_to_point 需要 ROS2 导航支持，当前仅记录目标") - return True - - def get_battery(self) -> Optional[float]: - """获取电池电压""" - if not self.is_connected(): - return None - try: - self._agv.set_auto_report_state(1) - msg = self._agv.get_auto_report_message() - if msg and len(msg) > 5: - return msg[5] # 电池电压 - except: - pass - return None - - def disconnect(self): - if self._agv: - self.stop() - self._agv = None - self._connected = False - - def __enter__(self): - self.connect() - return self - - def __exit__(self, *args): - self.disconnect() \ No newline at end of file diff --git a/agv_app/utils/config.py b/agv_app/utils/config.py deleted file mode 100644 index b8adbc8..0000000 --- a/agv_app/utils/config.py +++ /dev/null @@ -1,92 +0,0 @@ -""" -配置文件 - 所有可配置参数集中管理 -""" -import os - -# 基础路径（部署后对应 ~/work/agv_app） -BASE_DIR = os.path.dirname(os.path.abspath(__file__)) - -# ========== 网络配置（集中管理所有 IP 地址 — 修改此处即可全局生效）========== -AGV_HOST = "192.168.60.80" -ARM_HOST = "192.168.60.120" - -# ========== AGV 参数 ========== -AGV_CONFIG = { - "device": "/dev/agvpro_controller", - "baudrate": 10000000, - "move_speed": 1.0, - "turn_speed": 1.0, -} - -# ========== 机械臂 TCP 客户端 ========== -ARM_CONFIG = { - "host": ARM_HOST, - "port": 5002, - "timeout": 8, - "retry_times": 3, - "retry_interval": 1, -} - -# ========== 地图 ========== -MAP_CONFIG = { - "map_dir": "/home/elephant/agv_pro_ros2/src/agv_pro_navigation2/map/", - "map_file": "map.yaml", -} - -# ========== 摄像头 ========== -CAMERA_CONFIG = { - "device_index": 4, # AGV 摄像头 video4（标准彩色摄像头，V4L2后端） - "backend": "v4l2", # 使用 V4L2 后端获取标准彩色格式（640x480） - "qr_detect_interval": 0.5, - "capture_delay": 0.5, -} - -# ========== 机械臂摄像头流 ========== -ARM_CAMERA_CONFIG = { - "url": f"http://{ARM_HOST}:5003/api/camera/preview", - "snapshot_url": f"http://{ARM_HOST}:5003/api/camera/snapshot", -} - -# ========== HTTP 上传 ========== -UPLOAD_CONFIG = { - "url": "https://ts.zhijian168.com/prod-api/file/uploadImage", - "timeout": 30, - "max_retries": 3, -} - -# ========== Flask 服务器 ========== -SERVER_CONFIG = { - "host": "0.0.0.0", - "port": 5000, - "secret_key": "agv630_secret_key_2024", - "debug": False, -} - -# ========== 任务配置存储路径 ========== -DATA_DIR = os.path.join(BASE_DIR, "data") -os.makedirs(DATA_DIR, exist_ok=True) - -# ========== 关节角度范围限制 ========== -JOINT_LIMITS = { - "J1": (-180.0, 180.0), - "J2": (-270.0, 90.0), - "J3": (-150.0, 150.0), - "J4": (-260.0, 80.0), - "J5": (-168.0, 168.0), - "J6": (-174.0, 174.0), -} - -# ========== 机械臂默认速度 ========== -DEFAULT_ARM_SPEED = 1000 - -# ========== 状态定义 ========== -class State: - SETTING = "setting" - RUNNING = "running" - PAUSED = "paused" - IDLE = "idle" - -class PhotoType: - FRONT = "front" - BACK = "back" - NAMEPLATE = "nameplate" diff --git a/agv_app/utils/map_navigator.py b/agv_app/utils/map_navigator.py deleted file mode 100644 index 91131c3..0000000 --- a/agv_app/utils/map_navigator.py +++ /dev/null @@ -1,663 +0,0 @@ -""" -地图导航模块 - A* 路径规划 + Pure Pursuit 路径跟踪 -在已知地图上规划路径，控制 AGV 自动导航到目标坐标 - -依赖：numpy, cv2, Pillow（均已安装在 AGV 上） -不依赖：激光雷达、SLAM、Nav2 -""" - -import os -import math -import heapq -import time -import logging -import threading -import subprocess -import numpy as np -import cv2 -import yaml -from typing import List, Tuple, Optional, Dict -from enum import Enum - -logger = logging.getLogger(__name__) - -# ROS2 环境设置（与 agv_controller_ros2.py 保持一致） -ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash" - - -# ========== 坐标转换 ========== - -class CoordTransformer: - """地图世界坐标 ↔ 栅格坐标 双向转换""" - - def __init__(self, resolution: float, origin: List[float], width: int, height: int): - """ - Args: - resolution: 地图分辨率（米/像素） - origin: [x, y, yaw] 地图原点在世界坐标系中的位置 - width: 地图宽度（像素） - height: 地图高度（像素） - """ - self.resolution = resolution - self.origin = origin # [ox, oy, oyaw] - self.width = width - self.height = height - - def world_to_grid(self, wx: float, wy: float) -> Tuple[int, int]: - """世界坐标 → 栅格坐标 [col, row]""" - col = int((wx - self.origin[0]) / self.resolution) - row = int((wy - self.origin[1]) / self.resolution) - # ROS 地图 row=0 对应图像最上方（y 最大值），需要翻转 - row = self.height - 1 - row - return (col, row) - - def grid_to_world(self, col: int, row: int) -> Tuple[float, float]: - """栅格坐标 [col, row] → 世界坐标 [x, y]""" - # 翻转 row - actual_row = self.height - 1 - row - wx = col * self.resolution + self.origin[0] - wy = actual_row * self.resolution + self.origin[1] - return (wx, wy) - - def world_to_grid_center(self, wx: float, wy: float) -> Tuple[float, float]: - """世界坐标 → 栅格中心的世界坐标（对齐到栅格）""" - col, row = self.world_to_grid(wx, wy) - return self.grid_to_world(col, row) - - -# ========== A* 路径规划 ========== - -class AStarPlanner: - """A* 路径规划器，在栅格地图上规划最短路径""" - - # 8方向移动：右、左、下、上、右下、右上、左下、左上 - DIRECTIONS = [ - (1, 0), (-1, 0), (0, 1), (0, -1), - (1, 1), (1, -1), (-1, 1), (-1, -1) - ] - # 对角线移动的代价乘数（sqrt(2)） - DIR_COSTS = [1.0, 1.0, 1.0, 1.0, 1.414, 1.414, 1.414, 1.414] - - def __init__(self, occupancy_grid: np.ndarray, inflation_radius: int = 3): - """ - Args: - occupancy_grid: 栅格地图，0=空闲，255=障碍物 - inflation_radius: 障碍物膨胀半径（像素），AGV 有一定体积不能贴墙走 - """ - self.grid = occupancy_grid - self.height, self.width = occupancy_grid.shape - self.inflated = self._inflate(inflation_radius) - - def _inflate(self, radius: int) -> np.ndarray: - """膨胀障碍物区域""" - if radius <= 0: - return self.grid.copy() - kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * radius + 1, 2 * radius + 1)) - inflated = cv2.dilate(self.grid, kernel, iterations=1) - # 确保二值化 - inflated = np.where(inflated > 50, 255, 0).astype(np.uint8) - return inflated - - def plan(self, start: Tuple[int, int], goal: Tuple[int, int]) -> Optional[List[Tuple[int, int]]]: - """ - A* 路径规划 - - Args: - start: 起点栅格坐标 (col, row) - goal: 终点栅格坐标 (col, row) - - Returns: - 路径点列表 [(col, row), ...]，包含起点和终点；无法规划时返回 None - """ - # 边界检查 - if not self._is_valid(start) or not self._is_valid(goal): - logger.warning(f"起点或终点无效: start={start}, goal={goal}") - # 尝试找最近的可行点 - start = self._find_nearest_free(start) - goal = self._find_nearest_free(goal) - if start is None or goal is None: - logger.error("无法找到有效的起点或终点") - return None - - # 检查终点是否被障碍物包围 - if self.inflated[goal[1], goal[0]] > 50: - goal = self._find_nearest_free(goal) - - if goal is None: - logger.error("终点周围无可行区域") - return None - - # A* 算法 - open_set = [] - heapq.heappush(open_set, (0.0, start)) - came_from = {} - g_score = {start: 0.0} - closed_set = set() - - while open_set: - _, current = heapq.heappop(open_set) - - if current in closed_set: - continue - closed_set.add(current) - - if current == goal: - # 回溯路径 - path = [] - while current in came_from: - path.append(current) - current = came_from[current] - path.append(start) - path.reverse() - return path - - for i, (dx, dy) in enumerate(self.DIRECTIONS): - neighbor = (current[0] + dx, current[1] + dy) - - if neighbor in closed_set: - continue - - if not self._is_valid(neighbor): - continue - - if self.inflated[neighbor[1], neighbor[0]] > 50: - continue - - move_cost = self.DIR_COSTS[i] - tentative_g = g_score[current] + move_cost - - if tentative_g < g_score.get(neighbor, float('inf')): - came_from[neighbor] = current - g_score[neighbor] = tentative_g - f_score = tentative_g + self._heuristic(neighbor, goal) - heapq.heappush(open_set, (f_score, neighbor)) - - logger.warning("A* 无法找到路径") - return None - - def _heuristic(self, a: Tuple[int, int], b: Tuple[int, int]) -> float: - """对角线距离启发式""" - dx = abs(a[0] - b[0]) - dy = abs(a[1] - b[1]) - return max(dx, dy) + (1.414 - 1) * min(dx, dy) - - def _is_valid(self, pos: Tuple[int, int]) -> bool: - return 0 <= pos[0] < self.width and 0 <= pos[1] < self.height - - def _find_nearest_free(self, pos: Tuple[int, int], max_dist: int = 10) -> Optional[Tuple[int, int]]: - """在 pos 附近找最近的可行点""" - for r in range(1, max_dist + 1): - for dx in range(-r, r + 1): - for dy in range(-r, r + 1): - n = (pos[0] + dx, pos[1] + dy) - if self._is_valid(n) and self.inflated[n[1], n[0]] == 0: - return n - return None - - -# ========== 路径平滑 ========== - -def smooth_path(grid: np.ndarray, path: List[Tuple[int, int]], - weight_data: float = 0.3, weight_smooth: float = 0.5, - tolerance: float = 1e-5, max_iter: int = 500) -> List[Tuple[int, int]]: - """ - 路径平滑（梯度下降法） - 在障碍物约束下让路径更平滑，减少不必要的转向 - """ - if len(path) <= 2: - return path - - height, width = grid.shape - new_path = [list(p) for p in path] - - for iteration in range(max_iter): - change = 0.0 - for i in range(1, len(new_path) - 1): - for j in range(2): - old_val = new_path[i][j] - # 数据项：趋向原始路径点 - data_gradient = weight_data * (path[i][j] - new_path[i][j]) - # 平滑项：趋向邻居中点 - smooth_gradient = weight_smooth * ( - new_path[i - 1][j] + new_path[i + 1][j] - 2 * new_path[i][j] - ) - new_path[i][j] += data_gradient + smooth_gradient - - # 边界约束 - new_path[i][0] = max(0, min(width - 1, new_path[i][0])) - new_path[i][1] = max(0, min(height - 1, new_path[i][1])) - - # 障碍物约束 - col, row = int(round(new_path[i][0])), int(round(new_path[i][1])) - if 0 <= col < width and 0 <= row < height: - if grid[row, col] > 50: - new_path[i][j] = old_val # 回退 - - change += abs(new_path[i][j] - old_val) - - if change < tolerance: - break - - return [(int(round(p[0])), int(round(p[1]))) for p in new_path] - - -# ========== 路径降采样 ========== - -def downsample_path(path: List[Tuple[int, int]], min_dist: int = 3) -> List[Tuple[int, int]]: - """降采样路径，移除过近的点，减少 cmd_vel 发布频率""" - if len(path) <= 2: - return path - - result = [path[0]] - for p in path[1:]: - last = result[-1] - dist = math.hypot(p[0] - last[0], p[1] - last[1]) - if dist >= min_dist: - result.append(p) - # 确保终点包含在内 - if result[-1] != path[-1]: - result.append(path[-1]) - return result - - -# ========== Pure Pursuit 控制器 ========== - -class PurePursuitController: - """Pure Pursuit 路径跟踪控制器""" - - def __init__(self, lookahead_distance: float = 0.3, - max_linear_speed: float = 0.4, - max_angular_speed: float = 0.8, - goal_tolerance: float = 0.15, - slow_down_distance: float = 0.5): - """ - Args: - lookahead_distance: 前视距离（米），越大转弯越平缓 - max_linear_speed: 最大线速度 (m/s) - max_angular_speed: 最大角速度 (rad/s) - goal_tolerance: 到达目标容差（米） - slow_down_distance: 开始减速的距离（米） - """ - self.lookahead_distance = lookahead_distance - self.max_linear_speed = max_linear_speed - self.max_angular_speed = max_angular_speed - self.goal_tolerance = goal_tolerance - self.slow_down_distance = slow_down_distance - self.transformer: Optional[CoordTransformer] = None - - def set_transformer(self, transformer: CoordTransformer): - self.transformer = transformer - - def compute(self, current_pos: Tuple[float, float, float], - path_world: List[Tuple[float, float]]) -> Tuple[float, float, bool]: - """ - 计算控制量 - - Args: - current_pos: (x, y, yaw) 当前世界坐标 - path_world: 路径点列表 [(x, y), ...] 世界坐标 - - Returns: - (linear_x, angular_z, reached) 线速度、角速度、是否到达 - """ - if not path_world: - return (0.0, 0.0, True) - - x, y, yaw = current_pos - - # 检查是否到达终点 - goal = path_world[-1] - dist_to_goal = math.hypot(goal[0] - x, goal[1] - y) - if dist_to_goal < self.goal_tolerance: - return (0.0, 0.0, True) - - # 找前视点（lookahead point） - lookahead_point = self._find_lookahead_point(x, y, path_world) - - if lookahead_point is None: - # 已经越过最后一个点 - return (0.0, 0.0, True) - - lx, ly = lookahead_point - - # 转换到机器人坐标系 - dx = lx - x - dy = ly - y - - # 旋转到机器人坐标系（x 轴朝前） - local_x = dx * math.cos(yaw) + dy * math.sin(yaw) - local_y = -dx * math.sin(yaw) + dy * math.cos(yaw) - - # 弧长 = 角度 * 半径 → curvature = 2 * ly / L^2 - L = math.hypot(local_x, local_y) - if L < 1e-6: - return (0.0, 0.0, True) - - curvature = 2.0 * local_y / (L * L) - angular_z = curvature * self.max_linear_speed - - # 根据距离调整速度 - linear_x = self.max_linear_speed - if dist_to_goal < self.slow_down_distance: - ratio = max(0.15, dist_to_goal / self.slow_down_distance) - linear_x *= ratio - - # 限制角速度 - angular_z = max(-self.max_angular_speed, min(self.max_angular_speed, angular_z)) - - # 如果角度偏差太大，先原位转弯 - angle_to_goal = math.atan2(ly - y, lx - x) - yaw - angle_to_goal = math.atan2(math.sin(angle_to_goal), math.cos(angle_to_goal)) - - if abs(angle_to_goal) > math.pi / 3: - # 角度偏差 > 60°，先原位转弯 - linear_x = 0.0 - angular_z = max(-self.max_angular_speed, min(self.max_angular_speed, angle_to_goal * 1.5)) - - return (linear_x, angular_z, False) - - def _find_lookahead_point(self, x: float, y: float, - path: List[Tuple[float, float]]) -> Optional[Tuple[float, float]]: - """沿路径找到前视距离处的点""" - for i in range(len(path) - 1, -1, -1): - dist = math.hypot(path[i][0] - x, path[i][1] - y) - if dist >= self.lookahead_distance: - return path[i] - # 如果所有点都在前视距离内，返回终点 - return path[-1] if path else None - - -# ========== 导航器（核心模块） ========== - -class NavStatus(Enum): - IDLE = "idle" - PLANNING = "planning" - NAVIGATING = "navigating" - REACHED = "reached" - FAILED = "failed" - CANCELLED = "cancelled" - - -class MapNavigator: - """地图导航器 — 整合路径规划与路径跟踪""" - - def __init__(self, map_yaml_path: str): - """ - Args: - map_yaml_path: map.yaml 文件的绝对路径 - """ - self.map_yaml_path = map_yaml_path - self.transformer: Optional[CoordTransformer] = None - self.planner: Optional[AStarPlanner] = None - self.controller = PurePursuitController() - self.controller.set_transformer(self.transformer) - - # 导航状态 - self.status = NavStatus.IDLE - self._nav_thread: Optional[threading.Thread] = None - self._cancel_event = threading.Event() - - # 当前路径（世界坐标） - self.path_world: List[Tuple[float, float]] = [] - self.current_position = [0.0, 0.0, 0.0] # [x, y, yaw] - - # 加载地图 - self._load_map() - - def _load_map(self): - """加载地图 PGM + YAML""" - with open(self.map_yaml_path, 'r') as f: - meta = yaml.safe_load(f) - - map_dir = os.path.dirname(self.map_yaml_path) - pgm_path = os.path.join(map_dir, meta['image']) - - # 读取 PGM 灰度图 - img = cv2.imread(pgm_path, cv2.IMREAD_GRAYSCALE) - if img is None: - raise FileNotFoundError(f"无法读取地图文件: {pgm_path}") - - # ROS 地图：0=占用（障碍物），254=空闲，205=未知 - # 转为二值：空闲=0，障碍物=255 - self.occupancy = np.where(img <= 50, 255, 0).astype(np.uint8) - # 未知区域（205 附近）也视为障碍物 - self.occupancy = np.where((img > 50) & (img < 250), 255, self.occupancy) - - resolution = meta['resolution'] - origin = meta.get('origin', [0, 0, 0]) - height, width = img.shape - - self.transformer = CoordTransformer(resolution, origin, width, height) - self.planner = AStarPlanner(self.occupancy, inflation_radius=3) - self.controller.set_transformer(self.transformer) - - self._map_meta = meta - logger.info(f"地图加载完成: {width}x{height}, 分辨率 {resolution}m, 原点 {origin}") - - def get_odom(self) -> List[float]: - """从 /odom 话题获取当前位置 [x, y, yaw]""" - try: - cmd = f"timeout 5 ros2 topic echo /odom --once 2>/dev/null" - full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'" - result = subprocess.run( - full_cmd, shell=True, capture_output=True, text=True, timeout=6 - ) - if result.returncode == 0 and result.stdout: - yaml_str = result.stdout.split('---')[0] - data = yaml.safe_load(yaml_str) - if data: - pos = data.get("pose", {}).get("pose", {}).get("position", {}) - x, y = pos.get("x", 0.0), pos.get("y", 0.0) - orient = data.get("pose", {}).get("pose", {}).get("orientation", {}) - qz, qw = orient.get("z", 0.0), orient.get("w", 1.0) - yaw = math.atan2(2.0 * qw * qz, 1.0 - 2.0 * qz * qz) - self.current_position = [x, y, yaw] - return self.current_position - except Exception as e: - logger.debug(f"获取 odom 失败: {e}") - return self.current_position - - def _publish_cmd_vel(self, linear_x: float, angular_z: float): - """发布速度命令到 /cmd_vel""" - msg = ( - f'{{"linear": {{"x": {linear_x:.4f}, "y": 0.0, "z": 0.0}}, ' - f'"angular": {{"x": 0.0, "y": 0.0, "z": {angular_z:.4f}}}}}' - ) - full_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \"{msg}\" --once'" - try: - subprocess.run(full_cmd, shell=True, capture_output=True, text=True, timeout=3) - except subprocess.TimeoutExpired: - logger.warning("发布 cmd_vel 超时") - - def _stop_cmd_vel(self): - """发布停止命令""" - self._publish_cmd_vel(0.0, 0.0) - - def plan_path(self, goal_x: float, goal_y: float, - start_x: float = None, start_y: float = None) -> bool: - """ - 规划路径（不执行导航） - - Args: - goal_x, goal_y: 目标世界坐标（米） - start_x, start_y: 起点世界坐标（米），默认使用当前 odom - - Returns: - 是否规划成功 - """ - if self.transformer is None: - logger.error("地图未加载") - return False - - # 获取起点 - if start_x is None or start_y is None: - pos = self.get_odom() - start_x, start_y = pos[0], pos[1] - - # 坐标转换 - start_grid = self.transformer.world_to_grid(start_x, start_y) - goal_grid = self.transformer.world_to_grid(goal_x, goal_y) - - logger.info(f"规划路径: 起点(世界){start_x:.2f},{start_y:.2f} → (栅格){start_grid}") - logger.info(f" 终点(世界){goal_x:.2f},{goal_y:.2f} → (栅格){goal_grid}") - - # A* 规划 - path_grid = self.planner.plan(start_grid, goal_grid) - if path_grid is None: - logger.warning("路径规划失败") - return False - - # 路径平滑 - path_grid = smooth_path(self.planner.inflated, path_grid) - - # 降采样 - path_grid = downsample_path(path_grid, min_dist=2) - - # 转换为世界坐标 - self.path_world = [self.transformer.grid_to_world(c, r) for c, r in path_grid] - - logger.info(f"路径规划成功: {len(self.path_world)} 个路径点") - return True - - def navigate_to(self, goal_x: float, goal_y, blocking: bool = False) -> bool: - """ - 导航到目标点 - - Args: - goal_x, goal_y: 目标世界坐标（米） - blocking: 是否阻塞等待导航完成 - - Returns: - 非阻塞模式下返回 True（表示已启动），阻塞模式下返回是否到达 - """ - if self.status == NavStatus.NAVIGATING: - logger.warning("导航正在进行中，请先停止当前导航") - return False - - # 规划路径 - if not self.plan_path(goal_x, goal_y): - self.status = NavStatus.FAILED - return False - - # 启动导航线程 - self._cancel_event.clear() - self.status = NavStatus.NAVIGATING - self._nav_thread = threading.Thread( - target=self._navigate_thread, - args=(goal_x, goal_y), - daemon=True - ) - self._nav_thread.start() - - if blocking: - self._nav_thread.join() - return self.status == NavStatus.REACHED - - return True - - def _navigate_thread(self, goal_x: float, goal_y: float): - """导航线程""" - logger.info(f"开始导航 → 目标 ({goal_x:.2f}, {goal_y:.2f})") - - try: - # 转弯朝向第一个路径点 - self._initial_turn() - - # 跟踪路径 - last_cmd_time = time.time() - cmd_interval = 0.2 # cmd_vel 发布间隔（秒） - - while not self._cancel_event.is_set(): - pos = self.get_odom() - x, y, yaw = pos - - linear_x, angular_z, reached = self.controller.compute( - (x, y, yaw), self.path_world - ) - - if reached: - self._stop_cmd_vel() - self.status = NavStatus.REACHED - logger.info("✅ 已到达目标点") - return - - # 控制发布频率 - now = time.time() - if now - last_cmd_time >= cmd_interval: - self._publish_cmd_vel(linear_x, angular_z) - last_cmd_time = now - - time.sleep(0.05) # 50ms 控制循环 - - # 被取消 - self._stop_cmd_vel() - self.status = NavStatus.CANCELLED - logger.info("导航已取消") - - except Exception as e: - self._stop_cmd_vel() - self.status = NavStatus.FAILED - logger.error(f"导航异常: {e}") - - def _initial_turn(self): - """导航开始前，先原地转向朝向第一个路径点""" - if len(self.path_world) < 2: - return - - pos = self.get_odom() - x, y, yaw = pos - target = self.path_world[1] # 第一个路径点是当前位置，取第二个 - - angle_to_target = math.atan2(target[1] - y, target[0] - x) - yaw - angle_to_target = math.atan2(math.sin(angle_to_target), math.cos(angle_to_target)) - - if abs(angle_to_target) < 0.1: # < 6°，不需要转弯 - return - - logger.info(f"初始转向: {math.degrees(angle_to_target):.1f}°") - - # 分段旋转（避免一步到位导致超调） - steps = max(3, int(abs(angle_to_target) / 0.2)) - step_angle = angle_to_target / steps - step_time = abs(step_angle) / self.controller.max_angular_speed + 0.1 - - for _ in range(steps): - if self._cancel_event.is_set(): - return - angular = max(-self.controller.max_angular_speed, - min(self.controller.max_angular_speed, step_angle * 2)) - self._publish_cmd_vel(0.0, angular) - time.sleep(step_time) - - self._stop_cmd_vel() - time.sleep(0.2) # 稳定后继续 - - def stop(self): - """停止当前导航""" - if self.status == NavStatus.NAVIGATING: - self._cancel_event.set() - self._stop_cmd_vel() - if self._nav_thread and self._nav_thread.is_alive(): - self._nav_thread.join(timeout=3) - self.status = NavStatus.CANCELLED - - def get_status(self) -> dict: - """获取导航状态""" - pos = self.get_odom() - return { - "status": self.status.value, - "current_position": pos, - "path_length": len(self.path_world), - "path": self.path_world if self.status in (NavStatus.NAVIGATING, NavStatus.REACHED) else [] - } - - def get_path_preview(self, goal_x: float, goal_y: float) -> Optional[List[Tuple[float, float]]]: - """ - 预览路径（仅规划不执行），用于前端可视化 - - Returns: - 世界坐标路径列表，或 None（规划失败） - """ - if self.plan_path(goal_x, goal_y): - return self.path_world - return None diff --git a/arm/arm_server.py b/arm/arm_server.py new file mode 100644 index 0000000..efad839 --- /dev/null +++ b/arm/arm_server.py @@ -0,0 +1,406 @@ +""" +机械臂服务端 - 机械臂端主程序 +运行在 10.247.46.165 上，端口 5002 (TCP) + 5003 (视频流) +通过 TCP Socket 接收 AGV 发来的指令，转发给 RoboFlow (ElephantRobot) +同时通过 ffmpeg 提供 HTTP 视频流 +""" +import socket +import threading +import time +import logging +import os +import sys +import subprocess +import io +from PIL import Image +from flask import Flask, Response, jsonify +from werkzeug.serving import make_server + +# 添加当前目录到路径 +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +# 配置日志 +logging.basicConfig( + level=logging.INFO, + format="%(asctime)s [%(levelname)s] %(name)s: %(message)s", + handlers=[ + logging.StreamHandler(), + logging.FileHandler(os.path.expanduser("~/work/arm_server/server.log")) + ] +) +logger = logging.getLogger("arm_server") + +# ========== Flask HTTP 服务器 - 视频流 (ffmpeg) ========== +arm_video_app = Flask(__name__) + +ARM_CAMERA_INDEX = 0 # 机械臂端摄像头设备号 +_ffmpeg_proc = None +_ffmpeg_thread = None +_ffmpeg_lock = threading.Lock() +_frame_cond = threading.Condition() +_latest_frame = None +_latest_frame_ts = 0.0 +_stop_ffmpeg_reader = threading.Event() +_invalid_count = 0 # 连续无效帧计数 +_MAX_INVALID = 30 # 连续 30 帧无效 → 重启 ffmpeg +_MAX_BUF_SIZE = 2 * 1024 * 1024 # 2MB buffer 上限 + + +def _validate_jpeg(data): + """验证 JPEG 数据是否有效，返回 True/False""" + try: + Image.open(io.BytesIO(data)).verify() + return True + except Exception: + return False + + +def _stop_ffmpeg(): + """停止 ffmpeg 采集进程和读帧线程。""" + global _ffmpeg_proc + _stop_ffmpeg_reader.set() + if _ffmpeg_proc and _ffmpeg_proc.poll() is None: + _ffmpeg_proc.terminate() + try: + _ffmpeg_proc.wait(timeout=2) + except subprocess.TimeoutExpired: + _ffmpeg_proc.kill() + _ffmpeg_proc = None + + +def _frame_reader(): + """从 ffmpeg 的连续 MJPEG 输出中解析 JPEG 帧，校验有效性并缓存最新一帧。 + + 当摄像头 USB 掉线重连时，ffmpeg 会从失效 fd 读取垃圾数据， + 产生假 JPEG 帧（花屏）。这里通过 PIL 校验帧有效性， + 连续无效帧过多时自动重启 ffmpeg 恢复。 + """ + global _ffmpeg_proc, _latest_frame, _latest_frame_ts, _invalid_count + buf = b"" + while not _stop_ffmpeg_reader.is_set(): + proc = _ffmpeg_proc + if proc is None or proc.poll() is not None or proc.stdout is None: + time.sleep(0.1) + continue + chunk = proc.stdout.read(8192) + if not chunk: + if proc.poll() is not None: + break + time.sleep(0.02) + continue + buf += chunk + # 防止垃圾数据撑爆内存 + if len(buf) > _MAX_BUF_SIZE: + logger.warning(f"frame buffer 超过 {_MAX_BUF_SIZE} 字节，丢弃并重启 ffmpeg") + buf = b"" + _stop_ffmpeg() + continue + while True: + start = buf.find(b"\xff\xd8") + end = buf.find(b"\xff\xd9", start + 2) if start >= 0 else -1 + if start < 0: + buf = buf[-2:] + break + if end < 0: + buf = buf[start:] + break + frame = buf[start:end + 2] + buf = buf[end + 2:] + # JPEG 校验：摄像头掉线时帧数据会损坏 + if _validate_jpeg(frame): + with _frame_cond: + _latest_frame = frame + _latest_frame_ts = time.time() + _frame_cond.notify_all() + _invalid_count = 0 + else: + _invalid_count += 1 + if _invalid_count >= _MAX_INVALID: + logger.error(f"连续 {_MAX_INVALID} 帧无效，摄像头可能掉线，重启 ffmpeg") + _stop_ffmpeg() + _invalid_count = 0 + break # 跳出循环让 _ensure_ffmpeg 重建 + + +def _ensure_ffmpeg(): + """确保 ffmpeg 进程在运行，自动重启崩溃的进程""" + global _ffmpeg_proc, _ffmpeg_thread, _invalid_count + with _ffmpeg_lock: + if _ffmpeg_proc is not None and _ffmpeg_proc.poll() is None: + return + + _stop_ffmpeg_reader.set() + if _ffmpeg_proc and _ffmpeg_proc.poll() is None: + _ffmpeg_proc.terminate() + _stop_ffmpeg_reader.clear() + _invalid_count = 0 # 重置错误计数 + + logger.info(f"启动 ffmpeg 视频流 (Video{ARM_CAMERA_INDEX})") + _ffmpeg_proc = subprocess.Popen( + [ + "ffmpeg", + "-f", "v4l2", + "-input_format", "mjpeg", + "-framerate", "8", + "-video_size", "640x480", + "-i", f"/dev/video{ARM_CAMERA_INDEX}", + "-fflags", "nobuffer", + + "-analyzeduration", "0", + "-f", "mjpeg", + "-" + ], + stdout=subprocess.PIPE, + stderr=subprocess.DEVNULL, + ) + _ffmpeg_thread = threading.Thread(target=_frame_reader, daemon=True) + _ffmpeg_thread.start() + + +def _get_latest_frame(timeout: float = 3.0): + """返回缓存的最新 JPEG 帧；必要时等待首帧。""" + _ensure_ffmpeg() + deadline = time.time() + timeout + with _frame_cond: + while _latest_frame is None and time.time() < deadline: + _frame_cond.wait(timeout=0.2) + return _latest_frame + + +@arm_video_app.route("/api/camera/preview") +def arm_camera_preview(): + """机械臂摄像头 MJPEG 流，共用后台 ffmpeg 采集进程。""" + _ensure_ffmpeg() + + def generate(): + last_ts = 0.0 + try: + while True: + frame = _get_latest_frame(timeout=3.0) + if frame is None: + logger.warning("等待摄像头帧超时，重启 ffmpeg") + _stop_ffmpeg() + continue + with _frame_cond: + if _latest_frame_ts <= last_ts: + _frame_cond.wait(timeout=1.0) + frame = _latest_frame + last_ts = _latest_frame_ts + if frame: + yield (b"--frame\r\nContent-Type: image/jpeg\r\n\r\n" + frame + b"\r\n") + except Exception as e: + logger.error(f"视频流异常: {e}") + finally: + logger.info("视频流连接关闭") + + return Response(generate(), mimetype="multipart/x-mixed-replace; boundary=frame") + + +@arm_video_app.route("/api/camera/status") +def arm_camera_status(): + """摄像头状态""" + global _ffmpeg_proc + running = _ffmpeg_proc is not None and _ffmpeg_proc.poll() is None + return jsonify({ + "opened": running, + "frame_age": time.time() - _latest_frame_ts if _latest_frame_ts else None, + "invalid_count": _invalid_count + }) + + +@arm_video_app.route("/api/camera/restart", methods=["POST"]) +def arm_camera_restart(): + """重启视频流""" + global _latest_frame, _latest_frame_ts, _invalid_count + _stop_ffmpeg() + with _frame_cond: + _latest_frame = None + _latest_frame_ts = 0.0 + _invalid_count = 0 + _ensure_ffmpeg() + return jsonify({"ok": True}) + +@arm_video_app.route("/api/camera/snapshot") +def arm_camera_snapshot(): + """机械臂摄像头单帧 JPEG，从常驻视频流缓存读取最新帧。""" + frame = _get_latest_frame(timeout=3.0) + if frame: + r = Response(frame, mimetype="image/jpeg") + r.headers["Cache-Control"] = "no-cache, no-store, must-revalidate, max-age=0" + r.headers["Pragma"] = "no-cache" + r.headers["Expires"] = "0" + return r + logger.warning("snapshot failed: no cached frame") + return "", 500 + + +# ========== TCP 服务器 - 接收 AGV 指令 ========== +class AGVCommandServer: + """TCP 服务器，接收 AGV 发来的指令，通过 ElephantRobot 转发给 RoboFlow""" + + def __init__(self, elephant, host: str = "0.0.0.0", port: int = 5002): + self.host = host + self.port = port + self._sock: socket.socket = None + self._running = False + # 直接从外部注入已激活的 ElephantRobot 实例 + if elephant is None: + logger.warning("ElephantRobot 实例为空，命令将返回错误") + self._el = elephant + + def start(self): + self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self._sock.bind((self.host, self.port)) + self._sock.listen(5) + self._running = True + logger.info(f"=" * 50) + logger.info(f"机械臂服务端已启动，监听 {self.host}:{self.port}") + logger.info(f"等待 AGV 连接...") + logger.info(f"=" * 50) + + while self._running: + try: + self._sock.settimeout(1.0) + try: + client_sock, addr = self._sock.accept() + logger.info(f"AGV 已连接: {addr}") + threading.Thread(target=self._handle_client, args=(client_sock,), daemon=True).start() + except socket.timeout: + continue + except Exception as e: + if self._running: + logger.error(f"服务器异常: {e}") + break + + def _handle_client(self, client_sock: socket.socket): + try: + client_sock.settimeout(30) + buffer = "" + while self._running: + try: + data = client_sock.recv(4096) + if not data: + break + buffer += data.decode("utf-8") + while "\n" in buffer: + line, buffer = buffer.split("\n", 1) + line = line.strip() + if not line: + continue + response = self._execute_command(line) + client_sock.sendall((response + "\n").encode("utf-8")) + logger.info(f"CMD: {line} → {response}") + except socket.timeout: + continue + except Exception as e: + logger.error(f"客户端处理异常: {e}") + finally: + client_sock.close() + logger.info("AGV 客户端已断开") + + def _execute_command(self, cmd: str) -> str: + """通过 ElephantRobot.send_command 转发给 RoboFlow""" + if self._el is None: + return "ERROR: Robot not initialized" + try: + return self._el.send_command(cmd) + except Exception as e: + return f"ERROR: {e}" + + def stop(self): + self._running = False + if self._sock: + try: + self._sock.close() + except: + pass + logger.info("机械臂服务端已停止") + + +# ========== 入口 ========== +_elephant = None # 全局 ElephantRobot 实例 + +def power_on_arm(max_retries: int = 5) -> bool: + """通过 ElephantRobot 给机械臂上电并激活（带重试），返回 ElephantRobot 实例""" + global _elephant + from pymycobot import ElephantRobot + + for attempt in range(1, max_retries + 1): + try: + logger.info(f"正在通过 ElephantRobot 连接 RoboFlow (尝试 {attempt}/{max_retries})...") + el = ElephantRobot("127.0.0.1", 5001) + el.start_client() + logger.info("ElephantRobot start_client 成功，等待2秒...") + time.sleep(2) + + el._power_on() + logger.info("power_on 指令已发送，等待2秒...") + time.sleep(2) + + el.start_robot() + logger.info("start_robot 指令已发送，等待5秒...") + time.sleep(5) + logger.info("✅ 机械臂上电+激活 全部完成") + + # 保存到全局，确保后续复用 + _elephant = el + return True + except Exception as e: + logger.warning(f"⚠️ 第 {attempt} 次尝试失败: {e}") + if attempt < max_retries: + logger.info(f"等待 3 秒后重试...") + time.sleep(3) + else: + logger.error(f"❌ 所有 {max_retries} 次尝试均失败，将以 limited 模式运行") + return False + return False + + +def main(): + import signal + + # 先通过 ElephantRobot 给机械臂上电并激活 + power_on_arm() + + # 将全局 _elephant 传给指令服务器 + server = AGVCommandServer(_elephant, port=5002) + + # 启动 Flask 视频流服务（端口 5003） + arm_server_http = None + for attempt in range(5): + try: + arm_server_http = make_server("0.0.0.0", 5003, arm_video_app, threaded=True) + break + except OSError as e: + if attempt < 4 and "Address already in use" in str(e): + logger.warning(f"端口 5003 被占用（第{attempt+1}次），等待...") + time.sleep(3) + else: + raise + http_thread = threading.Thread(target=arm_server_http.serve_forever, daemon=True) + http_thread.start() + logger.info("机械臂视频流服务已启动: http://0.0.0.0:5003") + + def signal_handler(sig, frame): + logger.info("收到停止信号...") + global _ffmpeg_proc, _elephant + if _ffmpeg_proc: + _ffmpeg_proc.terminate() + server.stop() + arm_server_http.shutdown() + if _elephant: + try: + _elephant.stop_client() + except: + pass + sys.exit(0) + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + server.start() + + +if __name__ == "__main__": + main() diff --git a/arm_server/arm_server.service b/arm_server/arm_server.service new file mode 100644 index 0000000..4c83d9a --- /dev/null +++ b/arm_server/arm_server.service @@ -0,0 +1,18 @@ +[Unit] +Description=Arm Server (TCP 5002 + Camera 5003) +After=network-online.target +Wants=network-online.target + +[Service] +Type=simple +User=pi +WorkingDirectory=/home/pi/work/arm_server +ExecStartPre=/bin/sleep 5 +ExecStart=/usr/bin/python3 /home/pi/work/arm_server/arm_server.py +Restart=on-failure +RestartSec=5 +StandardOutput=append:/home/pi/work/arm_server/stdout.log +StandardError=append:/home/pi/work/arm_server/stderr.log + +[Install] +WantedBy=multi-user.target diff --git a/docs/AGV_机械臂_技术说明文档.md b/docs/AGV_机械臂_技术说明文档.md new file mode 100644 index 0000000..5d047bd --- /dev/null +++ b/docs/AGV_机械臂_技术说明文档.md @@ -0,0 +1,887 @@ +# AGV + 机械臂 移动拍摄平台 — 技术说明文档 + +> **版本**: V3.0 | **更新时间**: 2026-06-17 | **作者**: 自动生成 + +--- + +## 目录 + +1. [项目概述](#1-项目概述) +2. [系统架构](#2-系统架构) +3. [硬件环境与网络拓扑](#3-硬件环境与网络拓扑) +4. [核心模块详解](#4-核心模块详解) +5. [通信协议](#5-通信协议) +6. [完整 API 接口文档](#6-完整-api-接口文档) +7. [任务执行流程](#7-任务执行流程) +8. [数据配置格式](#8-数据配置格式) +9. [部署与运维](#9-部署与运维) +10. [关键决策与约束](#10-关键决策与约束) + +--- + +## 1. 项目概述 + +### 1.1 业务目标 + +自动巡检拍摄系统：AGV（Automated Guided Vehicle）搭载大象机器人 630 六轴机械臂 + Orbbec Gemini 深度相机，按 M×N 网格布局自动导航到每台待检机器前，识别机器二维码→匹配机型→按预设姿态拍摄正面/背面照片→上传至后端管理系统。 + +### 1.2 核心能力 + +| 能力 | 说明 | +|------|------| +| **自主导航** | 基于 ROS2 Humble + Nav2 导航栈，读取预建地图，精确导航至每个目标坐标 | +| **多姿态拍摄** | 每台机器支持自定义正/背面多姿态（机械臂6关节角度预设） | +| **二维码识别** | 机械臂摄像头（倒装）+ 双引擎识别（pyzbar + OpenCV QRCodeDetector）| +| **蛇形路径** | M×N 网格蛇形路径优化，相邻路径点高效串联，避免无效往返 | +| **报关单查验** | 集成外部报关系统，按报关单机器清单逐台核对，自动统计查验进度 | +| **照片上传** | 拍摄后即时上传至 Java 后端文件服务，附带 serialNumber + index | +| **双摄像头** | AGV Orbbec 深度相机 + 机械臂 USB 摄像头，物理翻转纠正 + 花屏自动检测 | +| **单步执行/错误处理** | 支持单步调试模式、错误弹窗中断/跳过 | + +### 1.3 技术栈 + +| 层级 | 技术 | +|------|------| +| **后端** | Python 3 + Flask 2.x（端口 5000） | +| **前端** | Vue 3（CDN）+ 原生 JS + HTML/CSS | +| **机器人控制** | ROS2 Humble + nav2_simple_commander | +| **机械臂** | RoboFlow 630 → TCP Socket（arm_server）| +| **导航** | Nav2 (Behavior Tree) + AMCL 定位 | +| **部署** | SSH + expect 脚本远程重启 | + +--- + +## 2. 系统架构 + +### 2.1 整体架构图 + +``` +┌────────────────────────────────────────────────────────────────┐ +│ AGV (Ubuntu 22.04) │ +│ │ +│ ┌──────────────────────────────────────────────────────────┐ │ +│ │ Flask Web 服务 (:5000) │ │ +│ │ │ │ +│ │ ┌──────────┐ ┌──────────┐ ┌───────────────┐ │ │ +│ │ │ 控制面板 │ │ 设置页 │ │ 任务运行页 │ │ │ +│ │ │ index │ │ setting │ │ running │ │ │ +│ │ └──────────┘ └──────────┘ └───────────────┘ │ │ +│ │ │ │ +│ │ ┌──────────────────────────────────────────────────────┐ │ │ +│ │ │ GlobalState (全局状态) │ │ │ +│ │ │ state / arm_client / agv_controller / qr_scanner │ │ │ +│ │ └──────────────────────────────────────────────────────┘ │ │ +│ │ │ │ +│ │ ┌─────────────────┐ ┌──────────────────────────────┐ │ │ +│ │ │ 98 个 API 端点 │ │ MissionExecutorV3 任务核 │ │ │ +│ │ │ RESTful JSON │ │ M×N 网格 + 蛇形路径 │ │ │ +│ │ └─────────────────┘ └──────────────────────────────┘ │ │ +│ └──────────────────────────────────────────────────────────┘ │ +│ │ +│ ┌─────────────┐ ┌─────────────┐ ┌──────────────────────┐ │ +│ │ AGVController │ │ ArmClient │ │ Nav2Navigator │ │ +│ │ (ROS2/cmd_vel)│ │ TCP Socket │ │ BasicNavigator API │ │ +│ └──────┬──────┘ └──────┬──────┘ └──────────┬───────────┘ │ +│ │ │ │ │ +│ ┌──────┴──────┐ ┌─────┴──────────┐ ┌───────┴──────────┐ │ +│ │ ROS2 Topics │ │ arm_server (:5002)│ │ Nav2 Action Srv │ │ +│ │ /cmd_vel │ │ RoboFlow 630 │ │ /amcl_pose │ │ +│ │ /odom │ │ │ │ /navigate_to_pose│ │ +│ └─────────────┘ └─────────────────┘ └──────────────────┘ │ +└────────────────────────────────────────────────────────────────┘ + │ │ + ▼ ▼ +┌──────────────┐ ┌──────────────────────────────────┐ +│ 机械臂 (Pi) │ │ 外部服务 (Java 后端) │ +│ arm_server.py │ │ zhijian168.com / 192.168.60.159 │ +│ :5002 TCP │ │ customsListPage / customsMachines│ +│ :5003 Camera │ │ profile/printer / file/uploadImage│ +└──────────────┘ └──────────────────────────────────┘ +``` + +### 2.2 核心文件清单 + +| 文件 | 行数 | 职责 | +|------|------|------| +| `app.py` | 2132 | Flask 主程序，98 个 API 端点，GlobalState 全局状态管理 | +| `config.py` | 114 | 集中配置（IP、端口、API密钥、环境切换） | +| `utils/mission_executor.py` | 1198 | 任务执行器 V3：蛇形路径、导航、QR扫描、拍照、上传 | +| `utils/agv_controller_ros2.py` | 216 | AGV 运动控制（ROS2 topic 发布 cmd_vel） | +| `utils/arm_client.py` | 170 | 机械臂 TCP 客户端（set_angles/jog/power_on） | +| `utils/nav2_navigator.py` | 350 | Nav2 导航器（BasicNavigator API + /amcl_pose 位置） | +| `utils/qr_scanner.py` | 170 | 二维码扫描（V4L2 + 绿屏修复 + 双引擎识别） | +| `utils/image_uploader.py` | 76 | 照片 HTTP 上传（multipart/form-data） | +| `templates/index.html` | - | AGV 控制页面（实时控制 + 双摄像头预览） | +| `templates/setting.html` | - | 配置页面（网格/机型/点位/报关单） | +| `templates/running.html` | - | 任务运行页（进度 + QR弹窗 + 查验状态） | +| `static/js/app.js` | - | 控制页交互逻辑 | +| `static/js/setting.js` | - | 设置页交互逻辑 | +| `static/js/running.js` | - | 运行页交互逻辑 + SSE 实时推送 | + +--- + +## 3. 硬件环境与网络拓扑 + +### 3.1 设备清单 + +| 设备 | 角色 | IP 地址 | SSH 凭证 | 关键软件 | +|------|------|---------|----------|----------| +| **AGV** | 主控 + 运动平台 | `192.168.60.80` | `elephant` / `Elephant` | ROS2 Humble, Nav2, Flask | +| **机械臂 Pi** | 机械臂 + 摄像头 | `192.168.60.120` | `pi` / `elephant` | arm_server.py, RoboFlow, ffmpeg | +| **Java 测试服务器** | 报关单/上传后端 | `192.168.60.159:8080` | - | Spring Boot | +| **生产服务器** | 正式环境 | `ts.zhijian168.com` | - | HTTPS + Nginx | + +### 3.2 AGV 硬件映射 + +| 设备 | Linux 路径 | 用途 | +|------|-----------|------| +| AGV 控制器 | `/dev/ttyCH341USB0` | AGV 底盘串口控制 | +| 雷达 | `/dev/ttyACM0` | LiDAR 传感器 | +| Orbbec Gemini | `/dev/video4` | 深度相机（彩色流 YUYV 640×480） | + +### 3.3 网络参数 + +| 参数 | 值 | 说明 | +|------|-----|------| +| Flask 监听 | `0.0.0.0:5000` | AGV Web 服务 | +| 机械臂 TCP | `5002` | arm_server 控制端口 | +| 机械臂摄像头 | `5003` | arm_server MJPEG 流 | +| ROS_DOMAIN_ID | `1` | DDS 发现域（Flask/Nav2/AGV 节点统一） | +| AGV 串口波特率 | `1000000` | 底盘通信 | + +--- + +## 4. 核心模块详解 + +### 4.1 GlobalState — 全局状态管理 + +```python +class GlobalState: + state: str # "idle" | "setting" | "running" | "paused" + arm_client: ArmClient # 机械臂 TCP 客户端实例 + agv_controller: AGVController # ROS2 AGV 控制器 + qr_scanner: QRScanner # AGV 摄像头二维码扫描器 + navigator: Nav2Navigator # 导航实例 + mission_config: dict # {rows, cols, grid[][], positions[{row,col,side,coords,poses}]} + machines_config: list # [{id, row, col, front:{coords,poses}, back:{coords,poses}}] + models_config: list # [{id, name, poses:[{id,name,photo_type,arm_angles,speed}]}] + qr_config: list # [{id, name, joint_angles, qr_value, model_id}] + inspection: dict # 查验状态 {customsId, customsName, items:[{inventoryCode,quantify,inspected}]} + current_customs: dict # 当前报关单 {id, name, machine_ids} +``` + +**状态转换图**： +``` + IDLE ──connect_all──▶ SETTING ──start_mission──▶ RUNNING + ▲ ▲ │ + │ │ ┌─────┼─────┐ + │ │ ▼ ▼ ▼ + └──disconnect── PAUSED ◀── error/stop ── COMPLETED +``` + +### 4.2 MissionExecutorV3 — 任务执行器核心 + +#### 类结构 + +``` +MissionExecutorV3 +├── 连接管理: connect_all() / disconnect_all() +├── 主流程: execute_mission(mission_config, machines, models, options) +│ ├── 蛇形路径: _build_snake_path(rows, cols, grid) → 路径列表 +│ ├── 导航: _navigate(point, label) → Nav2Navigator +│ ├── QR 扫描: _scan_qr_with_poses(qr_configs, machine_row) +│ │ ├── _decode_qr_from_arm() → pyzbar/OpenCV +│ │ └── _request_manual_qr(message) → 用户手动输入 +│ ├── 机型查询: _lookup_model(qr_value) → 报关单API查询 +│ ├── 拍照: _shoot(model, side, row, col, qr_value, machine_row) +│ │ ├── _capture_arm_photo() → 机械臂摄像头 +│ │ └── _upload_photo_bytes() → HTTP上传 +│ └── 返回原点: _return_to_origin() +├── 控制: pause() / resume() / stop() +├── 单步执行: set_step_choice("confirm"|"retry"|"abort") +└── 错误处理: set_error_choice("skip"|"abort") +``` + +#### 蛇形路径算法 + +``` +假设 2行 × 5列，有机器位置: (0,0)(0,1)(0,2)(0,3)(0,4)(1,0)(1,1)(1,2)(1,3)(1,4) + +蛇形路径（按点位行 pr 遍历）: + pr=0 (1排正面): (0,0)→(0,1)→(0,2)→(0,3)→(0,4) [左→右] + pr=1 (1排背面 + 2排正面): (1,4)→(1,3)→(1,2)→(1,1)→(1,0) [右→左] + pr=2 (2排背面): (2,0)→(2,1)→(2,2)→(2,3)→(2,4) [左→右] + +PR 为奇数时列序反向。 + +同一点位同时服务上一行背面和下一行正面时，先执行背面，再执行正面。 +镜像规则：机器行号为奇数时，所有 J1 关节角度取反（镜像）。仅 J1 取反！ +``` + +#### 任务步骤控制开关 + +前端执行任务时可选择性开启/关闭步骤： + +| 开关 | 字段 | 默认 | 说明 | +|------|------|------|------| +| 机械臂初始化 | `arm_init` | true | 每个点位移到后恢复默认姿态 | +| AGV 移动 | `agv_move` | true | 导航到目标坐标 | +| 二维码识别 | `qr_scan` | true | 扫描机器二维码 | +| 正面拍照 | `front_photo` | true | 正面姿态组拍摄 | +| 背面拍照 | `back_photo` | true | 背面姿态组拍摄 | +| AGV 速度 | `agv_speed` | 1.0 | m/s | +| 机械臂速度 | `arm_speed` | 1000 | RoboFlow 速度参数 | + +### 4.3 AGVController — ROS2 运动控制 + +```python +class AGVController: + def connect() # 检查 /odom topic 是否存在 + def is_connected() # 连接状态 + def move_forward() # 前进 (linear.x > 0) + def move_backward() # 后退 (linear.x < 0) + def turn_left() # 左转 (angular.z > 0) + def turn_right() # 右转 (angular.z < 0) + def move_left_lateral() # 左横移 (linear.y > 0) + def move_right_lateral() # 右横移 (linear.y < 0) + def stop() # 停止 (全 0) + def get_position() # 从 /odom 获取位置 [x, y, yaw] + def get_battery() # 获取电压 +``` + +**原理**: 通过 `subprocess` 执行 `ros2 topic pub /cmd_vel geometry_msgs/msg/Twist` 发布速度指令。`--once` 参数发布一次后退出，底层 AGV 驱动收到后会持续执行直到收到下一条指令（或发送零值停止）。 + +**ROS 环境**: `source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1` + +### 4.4 ArmClient — 机械臂 TCP 客户端 + +```python +class ArmClient: + def connect() # TCP 连接到 arm_server:5002 + def send_command(cmd) # 发送文本命令，接收响应 + def get_angles() # → [J1..J6] 当前关节角度 + def set_angles(angles, speed) # 设置全部 6 关节角度 + def set_angle(joint, angle, speed) # 设置单个关节 + def jog_angle(joint, direction, speed) # 连续调节（-1/0/1） + def get_coords() # → [x, y, z, rx, ry, rz] + def power_on() / state_on() / state_off() # 上电控制 + def state_check() / check_running() # 状态查询 + def wait_done(timeout) # 等待命令执行完成 + def task_stop() # 紧急停止 +``` + +**通信协议**: 文本行协议（`\n` 分隔）。 +- **请求**: `command_name(param1,param2,...)\n` +- **响应**: `command_name:result` 或 `ok` + +**关节范围** (机械臂 630): +| 关节 | 范围 | +|------|------| +| J1 | ±180° | +| J2 | ±90° | +| J3 | ±90° | +| J4 | ±180° | +| J5 | ±90° | +| J6 | ±180° | + +### 4.5 Nav2Navigator — 自主导航 + +```python +class Nav2Navigator: + def navigate_to_pose(x, y, yaw, timeout_sec, blocking) + # 使用 BasicNavigator.goToPose() 发送导航目标 + # 子线程中轮询 isTaskComplete()，超时自动取消 + def navigate_through_poses(poses, timeout_per_pose, blocking) + # 多路径点连续导航 + def stop() # 取消当前导航 + def get_status() # {status, current_position, nav2_available} + def get_current_position() # 从 /amcl_pose topic 获取 [x,y,yaw] +``` + +**工作原理**: +1. 使用 `nav2_simple_commander.BasicNavigator`（官方 Python API） +2. 在子线程中初始化 `rclpy`，构造 `PoseStamped` 消息并调用 `goToPose()` +3. 轮询 `isTaskComplete()` 查看导航是否完成 +4. 超时时调用 `cancelTask()` 取消 +5. 位置反馈从 `/amcl_pose`（AMCL 定位结果）而非 `/odom`（里程计）获取，避免累积漂移 + +**返回原点机制**: `_return_to_origin()` 导航到 `(0, 0)`，超时 180 秒，最多重试 3 次。 + +### 4.6 QRScanner — 二维码识别 + +```python +class QRScanner: + def open() # 打开摄像头（V4L2，device_index=4） + def read_frame() # 读取一帧（带超时保护） + def detect_qr(frame) # 双引擎：pyzbar > OpenCV QRCodeDetector + def scan_once() # 单次扫描 + def scan_with_retry(max_attempts, interval) # 多次重试 +``` + +**双引擎策略**: +1. **pyzbar**（优先）: 识别率更高，支持多种条码格式 +2. **OpenCV QRCodeDetector**（兜底）: pyzbar 失败时启用 + +**绿屏/花屏修复**: `_fix_frame()` 方法检测 YUYV 格式未转换导致的绿屏（G 通道全满），自动做 `COLOR_YUV2BGR_YUYV` 转换。全黑帧直接丢弃。 + +### 4.7 ImageUploader — 照片上传 + +```python +class ImageUploader: + def upload(image_path, serial_number, photo_index, photo_type) + def upload_batch(image_paths, serial_number, start_index) +``` + +**上传协议**: +- **方法**: HTTP POST（multipart/form-data） +- **URL**: `{ZHIJIAN_BASE_URL}{API_PREFIX}/file/uploadImage` + - 正式: `https://ts.zhijian168.com/prod-api/file/uploadImage` + - 测试: `http://192.168.60.159:8080/file/uploadImage` +- **字段**: `file` (MultipartFile), `serialNumber` (String), `index` (Integer) +- **认证**: `Authorization: Bearer ` +- **重试**: 最多 3 次，间隔 2 秒 + +--- + +## 5. 通信协议 + +### 5.1 Flask ↔ 前端 + +- **协议**: HTTP RESTful JSON +- **端口**: `5000` +- **格式**: `{"ok": bool, ...data}` + +### 5.2 Flask ↔ AGV (ROS2) + +```bash +# 发布速度指令 +ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" --once + +# 获取位置 (AMCL) +ros2 topic echo /amcl_pose --once +``` + +### 5.3 Flask ↔ 机械臂 (TCP) + +``` +请求: set_angles(-90.33,-90.08,0.16,-90.57,0.09,22.23,1000)\n +响应: set_angles:ok + +请求: get_angles()\n +响应: get_angles:[-90.33,-90.08,0.16,-90.57,0.09,22.23] +``` + +### 5.4 Flask ↔ Java 后端 + +| 接口 | 方法 | URL 路径 | 说明 | +|------|------|---------|------| +| 报关单列表 | GET | `/zhijian/integration/customsListPage` | ?pageNum=&pageSize= | +| 机器列表 | GET | `/zhijian/integration/customsMachines` | ?customsId= | +| 机型查询 | GET | `/zhijian/profile/printer` | ?serialNumber= | +| 文件上传 | POST | `/file/uploadImage` | multipart/form-data | + +**认证**: 所有请求携带 `Authorization: Bearer ` 头。 + +--- + +## 6. 完整 API 接口文档 + +### 6.1 系统状态 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/status` | GET | 全局状态（连接/地图/任务统计） | - | +| `/api/system/connect` | POST | 一次性连接所有设备 | - | +| `/api/system/disconnect` | POST | 断开所有设备 | - | +| `/api/device/connect` | POST | 连接单个设备 | `{"device":"agv\|arm\|camera\|arm_camera"}` | + +### 6.2 AGV 控制 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/agv/move` | POST | 控制移动 | `{"direction":"forward\|backward\|left\|right\|left_lateral\|right_lateral\|stop","speed":1.0}` | +| `/api/agv/position` | GET | 获取位置+电量 | - | +| `/api/agv/stop` | POST | 紧急停止 | - | +| `/api/agv/reset` | POST | 撞物体后复位 | - | + +### 6.3 机械臂控制 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/arm/get_angles` | GET | 获取当前6关节角度 | - | +| `/api/arm/set_angles` | POST | 设置全部关节 | `{"angles":[],"speed":1000}` | +| `/api/arm/set_angle` | POST | 设置单个关节 | `{"joint":"J1","angle":90,"speed":500}` | +| `/api/arm/jog` | POST | 连续调节关节 | `{"joint":"J1","direction":1\|-1\|0,"speed":500}` | +| `/api/arm/get_coords` | GET | 获取末端坐标 | - | +| `/api/arm/power_on` | POST | 上电 | - | +| `/api/arm/state_on` | POST | 激活 | - | +| `/api/arm/state_off` | POST | 去激活 | - | +| `/api/arm/state_check` | GET | 检查状态 | - | + +### 6.4 摄像头 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/camera/preview` | GET | AGV 摄像头 MJPEG 流 | - | +| `/api/camera/refresh` | GET | AGV 摄像头单帧 JPEG | - | +| `/api/camera/capture` | GET | 拍摄一张照片保存本地 | - | +| `/api/camera/arm_refresh` | GET | 机械臂摄像头单帧（翻转+花屏检测） | - | +| `/api/camera/arm_preview` | GET | 机械臂摄像头 MJPEG 代理流 | - | +| `/api/camera/qr_scan` | GET | AGV 摄像头扫码一次 | - | +| `/api/camera/capabilities` | GET | 摄像头能力信息 | - | + +### 6.5 地图导航 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/map/load` | POST | 加载地图文件 | `{"map_dir":"...","map_file":"map.yaml"}` | +| `/api/map/save` | POST | 保存地图配置 | `{"map_dir":"...","map_file":"map.yaml"}` | +| `/api/map/image` | GET | 获取地图 PNG 图像 | - | +| `/api/map/meta` | GET | 获取地图元数据（分辨率/原点/尺寸） | - | +| `/api/navigate/to` | POST | 导航到目标坐标 | `{"x":1.0,"y":2.0,"yaw":0.0}` | +| `/api/navigate/stop` | POST | 停止导航 | - | +| `/api/navigate/cancel` | POST | 取消导航 | - | +| `/api/navigate/status` | GET | 获取导航状态 | - | +| `/api/navigate/path` | POST | 预览路径（Nav2 不可用） | `{"x":1.0,"y":2.0}` | + +### 6.6 任务执行 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/mission/start` | POST | 开始执行任务 | `{"single_step":false,"arm_init":true,"agv_move":true,"qr_scan":true,"front_photo":true,"back_photo":true,"agv_speed":1.0,"arm_speed":1000}` | +| `/api/mission/stop` | POST | 停止任务 | - | +| `/api/mission/pause` | POST | 暂停任务 | - | +| `/api/mission/resume` | POST | 恢复任务 | - | +| `/api/mission/report` | GET | 获取执行报告 | - | +| `/api/mission/state` | GET | 任务实时状态（步骤/进度/查验/QR消息） | - | +| `/api/mission/log` | GET | 实时日志 | - | +| `/api/mission/manual-qr` | POST | 手动输入二维码（弹窗提交） | `{"qr":"BG042110276"}` | +| `/api/mission/error-skip` | POST | 错误弹窗：跳过 | - | +| `/api/mission/error-abort` | POST | 错误弹窗：中断 | - | +| `/api/mission/singlestep/confirm` | POST | 单步确认 | - | +| `/api/mission/singlestep/retry` | POST | 单步重试 | - | +| `/api/mission/singlestep/abort` | POST | 单步中断 | - | + +### 6.7 任务配置 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/mission/config` | GET | 获取网格配置+空位矩阵 | - | +| `/api/mission/config` | POST | 设置网格配置 | `{"rows":2,"cols":5,"grid":[[],...],"arm_initial_pose":[]}` | +| `/api/mission/position` | GET | 获取 AGV 当前位置（设置点位用） | - | +| `/api/mission/init_pose` | POST | 将 AMCL 初始位置设为 (0,0,0) | - | +| `/api/mission/positions` | GET | 获取所有点位坐标 | - | +| `/api/mission/positions` | POST | 保存/更新单点位 | `{"row":0,"col":0,"side":"front","coords":[],"poses":[]}` | +| `/api/mission/machines` | GET | 获取所有机器配置 | - | +| `/api/mission/machines` | POST | 批量保存机器配置 | `{"machines":[...]}` | +| `/api/mission/machines/add` | POST | 添加单台机器 | `{"row":0,"col":0,"front":{},"back":{}}` | +| `/api/mission/machines/` | PUT | 更新机器配置 | | +| `/api/mission/machines/` | DELETE | 删除机器配置 | | +| `/api/mission/poses//` | GET | 获取机器指定侧姿态 | - | +| `/api/mission/poses//` | POST | 添加姿态到机器 | `{"arm_angles":[],"speed":500}` | +| `/api/mission/poses///` | DELETE | 删除姿态 | - | +| `/api/mission/qr_scan/` | POST | AGV 摄像头扫码关联机器 | - | +| `/api/mission/generate_sequence` | GET | 生成蛇形拍摄序列预览 | - | + +### 6.8 机型配置 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/models/list` | GET | 获取所有机型 | - | +| `/api/models/add` | POST | 添加机型 | `{"name":"机型1","serial_prefix":"BG"}` | +| `/api/models/` | POST | 更新机型 | - | +| `/api/models/` | DELETE | 删除机型 | - | +| `/api/models/poses/add` | POST | 添加姿态到机型 | `{"model_id":"xxx","name":"正1","photo_type":"front","arm_angles":[]}` | +| `/api/models//poses` | GET | 获取机型姿态列表 | - | +| `/api/models//poses/` | PUT | 更新姿态 | - | +| `/api/models//poses/` | DELETE | 删除姿态 | - | + +### 6.9 二维码配置 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/qr/configs` | GET | 获取所有二维码配置 | - | +| `/api/qr/configs` | POST | 添加二维码配置 | `{"name":"二维码1","joint_angles":[]}` | +| `/api/qr/configs/` | PUT | 更新二维码配置 | - | +| `/api/qr/configs/` | DELETE | 删除二维码配置 | - | +| `/api/qr/configs//read-angles` | POST | 读取当前臂角度写入配置 | - | +| `/api/qr/scan/` | POST | 机械臂摄像头扫码保存 | - | + +### 6.10 报关单与查验 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/customs/list` | GET | 报关单列表（代理） | `?pageNum=1&pageSize=50` | +| `/api/customs/machines` | GET | 报关单机器列表（代理） | `?customsId=xxx` | +| `/api/customs/selected` | POST | 设定当前报关单 | `{"id":"xxx","name":"xxx","machine_ids":[]}` | +| `/api/customs/selected` | GET | 获取当前报关单 | - | +| `/api/customs/printer` | GET | 查询机型+更新查验计数 | `?serialNumber=xxx` | +| `/api/customs/inspection/start` | POST | 开始查验 | `{"customsId":"xxx"}` | +| `/api/customs/inspection` | GET | 获取查验状态 | - | +| `/api/customs/inspection/end` | POST | 结束查验 | - | +| `/api/customs/inspection/update` | POST | 直接更新计数 | `{"inventoryCode":"xxx"}` | + +### 6.11 环境切换 API + +| 路由 | 方法 | 说明 | 参数 | +|------|------|------|------| +| `/api/config/mode` | GET | 获取当前环境 | - | +| `/api/config/mode` | POST | 切换测试/正式环境 | `{"test_mode":true}` | + +**环境差异**: +| 项目 | 测试环境 | 正式环境 | +|------|---------|---------| +| Base URL | `http://192.168.60.159:8080` | `https://ts.zhijian168.com` | +| API 前缀 | 空 | `/prod-api` | +| 上传地址 | `http://192.168.60.159:8080/file/uploadImage` | `https://ts.zhijian168.com/prod-api/file/uploadImage` | + +--- + +## 7. 任务执行流程 + +### 7.1 完整生命周期 + +``` +[1] 前端设置页配置 + ├── 加载地图 → 设置 M×N 网格尺寸（rows/cols） + ├── 标注空位（Machine Toggle 切换每个单元格有/无机器） + ├── 逐点位标定坐标（AGV 开到机器前→读取位置→保存） + ├── 配置二维码扫描角度（机械臂对准二维码位置） + ├── 配置机型姿态组（正/背面，每面多角度） + └── 连接设备（AGV/机械臂/摄像头） + +[2] 报关单查验 + ├── 选择报关单 → 开始查验 + └── 系统按 inventoryCode 聚合统计各机型待查验数量 + +[3] 启动任务 + ├── POST /api/mission/start（可选单步模式+步骤开关） + └── MissionExecutorV3.execute_mission() 在新线程中运行 + +[4] 逐点位蛇形执行 + For each 点位 (pr, c) in 蛇形路径: + ├── [可选] 恢复机械臂初始姿态 + ├── [可选] 导航到该点位坐标 + │ └── Nav2Navigator.navigate_to_pose() → BasicNavigator.goToPose() + │ + ├── 背面操作（如果 pr>0 且 (pr-1,c) 有机器） + │ ├── 切换到 QR 扫描姿态（可选） + │ ├── 扫描二维码 → 查机型 → [可选] 拍照 + │ └── 上传照片 + 更新查验计数 + │ + └── 正面操作（如果 pr= quantify） + │ └── 返回机型名称 + │ + └── _shoot(model, "front"): + ├── 逐姿态设置关节角度 + 等待就位 + ├── _capture_arm_photo() → 机械臂摄像头拍照 + ├── _upload_photo_bytes() → HTTP上传 + └── 更新查验计数 + +[5] 任务完成 + ├── _return_to_origin() → 导航回 (0,0) + └── 生成执行报告 +``` + +### 7.2 QR 扫描流程详解 + +``` +_scan_qr_with_poses(qr_configs, machine_row): + 1. 逐 QR 配置尝试 + ├── set_angles(qr_config.joint_angles) → 机械臂移到扫码位 + ├── _wait_arm_ready() → 等待到位（容差 2°） + └── _decode_qr_from_arm(): + ├── HTTP GET 机械臂摄像头单帧 + ├── 花屏检测 (_is_corrupted_jpeg) + ├── pyzbar.decode() → 识别成功 + └── OpenCV QRCodeDetector → 兜底 + + 2. 如果识别失败: + ├── 报错日志 + 弹窗 _request_manual_qr() + └── 强制用户扫描/输入（不可跳过，仅任务停止可退出） + + 3. 如果机型不在报关单 (_lookup_model 返回 matched=null): + ├── 弹窗 _request_manual_qr() 强制重新输入 + └── 循环直到匹配或任务停止 + + 4. 如果已查验数量 ≥ 报关单数量 (_lookup_model 检测超量): + ├── 弹窗 _request_manual_qr() 强制重新输入 + └── 循环直到不超量或任务停止 +``` + +### 7.3 拍照流程详解 + +``` +_shoot(model, side, row, col, qr_value, machine_row): + 1. 过滤姿态: 只取 photo_type == side 的姿态 + 2. 镜像规则: machine_row % 2 == 1 → J1 = -J1 + 3. 逐姿态执行: + ├── set_angles(pose.arm_angles, speed) + ├── _wait_arm_ready() → 等待姿态稳定 + ├── _capture_arm_photo(): + │ ├── HTTP GET 机械臂摄像头 JPG + │ ├── 花屏检测 + │ └── 保存到 /home/elephant/photos/ + └── _upload_photo_bytes(): + ├── POST multipart/form-data + ├── serialNumber = qr_value + ├── index = next_upload_index（全局递增，从1开始） + └── 重试3次 + 4. 日志: "拍照完成 (机型=Mxx, 面=正面, 位置=r-c)" +``` + +### 7.4 错误处理 + +| 场景 | 触发条件 | 处理方式 | +|------|---------|---------| +| 导航失败 | Nav2 超时/返回 failed | 错误弹窗（跳过/中断） | +| QR 识别失败 | 所有姿态尝试均未识别 | 手动输入弹窗（不能跳过） | +| 机型不在报关单 | printer 返回空 matchedItem | 手动输入弹窗（不能跳过） | +| 查验超量 | inspected >= quantify | 手动输入弹窗（不能跳过） | +| 拍照失败 | HTTP 请求/文件损坏 | 记录日志，继续下一张 | +| 上传失败 | HTTP 超时/401/非200 | 重试3次，记录日志 | +| 机械臂超时 | _wait_arm_ready 15秒超时 | 记录实际偏差，继续执行 | + +--- + +## 8. 数据配置格式 + +### 8.1 任务网格配置 (mission_config.json) + +```json +{ + "rows": 2, + "cols": 5, + "grid": [[true, true, true, true, true], + [true, true, true, true, true]], + "positions": [ + {"row": 0, "col": 0, "side": "front", "coords": [0.54, -1.32, -0.05], "poses": []}, + {"row": 1, "col": 0, "side": "back", "coords": [0.65, -3.63, -3.06], "poses": []} + ], + "arm_initial_pose": [-90.33, -90.08, 0.16, -90.57, 0.09, 22.23] +} +``` + +- `grid[r][c]` = `true` 表示该位置有机器 +- `positions` 中 `row=pr` 表示点位行（非机器行），机器行 `mr = pr` (正面) 或 `mr = pr-1` (背面) +- `coords = [x, y, yaw]` 地图坐标和朝向 + +### 8.2 机器配置 (machines_config.json) + +```json +[{ + "id": "m_0_0", + "row": 0, "col": 0, + "front": { + "coords": [0.54, -1.32, -0.05], + "poses": [{"id":"pose_xxx","name":"正1","arm_angles":[...],"speed":500}] + }, + "back": { + "coords": [0.65, -3.63, -3.06], + "poses": [{"id":"pose_xxx","name":"背1","arm_angles":[...],"speed":500}] + } +}] +``` + +### 8.3 机型配置 (models_config.json) + +```json +[{ + "id": "m_1778767289", + "name": "MXM465N", + "serial_prefix": "BG", + "poses": [ + { + "id": "pose_xxx1", + "name": "正面姿态1", + "photo_type": "front", + "arm_angles": [-93.59, -184.34, 50.58, -38.33, -85.15, 20.40], + "speed": 500 + }, + { + "id": "pose_xxx2", + "name": "背面姿态1", + "photo_type": "back", + "arm_angles": [15.86, -161.13, 138.0, -162.0, 168.0, 15.65], + "speed": 500 + } + ] +}] +``` + +- `photo_type`: `"front"` / `"back"` / `"nameplate"` +- `arm_angles`: `[J1, J2, J3, J4, J5, J6]` 单位为度 + +### 8.4 二维码扫描姿态 (qr_config.json) + +```json +[{ + "id": "qr_001", + "name": "正面扫码位", + "joint_angles": [-89.80, -2.01, -87.18, -82.50, -93.32, 20.40], + "qr_value": "", + "model_id": "" +}] +``` + +--- + +## 9. 部署与运维 + +### 9.1 环境要求 + +**AGV (主控)**: +- Ubuntu 22.04 (ROS2 Humble) +- Python 3.8+ +- OpenCV (cv2), Flask, requests, numpy, pyzbar, PyYAML +- ROS2 Humble + nav2_simple_commander + +**机械臂 (Pi)**: +- arm_server.py（TCP 服务器端口 5002） +- arm_camera.py（MJPEG 服务器端口 5003） +- RoboFlow（大象机器人 SDK） + +### 9.2 启动流程 + +```bash +# === 机械臂端 (Pi) === +# 1. 启动 arm_server (TCP 5002) + arm_camera (MJPEG 5003) +sudo systemctl start arm_server + +# === AGV 端 === +# 2. 启动 ROS2 导航栈 +cd ~/agv_pro_ros2 +ros2 launch agv_pro_navigation2 navigation2.launch.py + +# 3. 启动 Flask +cd ~/work/agv_app +python3 app.py # 监听 0.0.0.0:5000 +``` + +### 9.3 部署命令 + +```bash +# 本地 → AGV SCP 部署（逐个文件） +scp app.py elephant@192.168.60.80:/home/elephant/work/agv_app/app.py +scp utils/mission_executor.py elephant@192.168.60.80:/home/elephant/work/agv_app/utils/mission_executor.py + +# 部署后验证远程文件 +ssh elephant@192.168.60.80 "grep 'def _lookup_model' /home/elephant/work/agv_app/utils/mission_executor.py" + +# 重启 Flask（使用 expect 脚本，避免 sshpass+nohup SIGTERM 问题） +# 方式一：手动 killing +ssh elephant@192.168.60.80 "pkill -f 'python3 app.py'; sleep 1; cd /home/elephant/work/agv_app && nohup python3 app.py > /tmp/flask.log 2>&1 &" + +# 清空 Python 缓存（关键！修改后必须清） +ssh elephant@192.168.60.80 "find /home/elephant/work/agv_app -name '*.pyc' -delete; find /home/elephant/work/agv_app -name '__pycache__' -type d -exec rm -rf {} +" +``` + +### 9.4 关键运维经验 + +| 问题 | 根因 | 解决方案 | +|------|------|---------| +| Flask 模板/JS 不生效 | 模板缓存 | 重启 Flask 服务 | +| Python 修改不生效 | `__pycache__` 缓存 | 删除所有 .pyc 和 __pycache__ | +| V4L2 摄像头无响应 | 设备独占 | kill 残留进程后重开 | +| ROS2 节点互相不可见 | ROS_DOMAIN_ID 不一致 | 统一设为 1 | +| 导航 DDS 发现失败 | FastRTPS 共享内存残留 | `rm -f /dev/shm/fastrtps_*` | +| 机械臂摄像头花屏 | USB 掉线致 ffmpeg 读失效 fd | arm_server 添加 JPEG 校验+自动重连 | +| Flask SIGTERM 被杀 | sshpass+nohup 触发 | 用 expect 脚本重启 | +| 照片上传 405 | 缺少 `/prod-api` 前缀 | config.py 动态拼接 API_PREFIX | + +--- + +## 10. 关键决策与约束 + +### 10.1 架构决策 + +| 决策 | 原因 | +|------|------| +| **agv_controller 用 ROS2 CLI 而非 rclpy** | 避免 rclpy 初始化与 Flask 多线程冲突 | +| **Nav2 用 BasicNavigator API 而非 subprocess** | 原生 API 更可靠（subprocess 的 stdin pipe 有 Humble bug） | +| **机械臂用 TCP Socket 而非 pymycobot** | pymycobot 存在死锁问题 | +| **位置源用 /amcl_pose 而非 /odom** | /odom 累积漂移，/amcl_pose 有地图校正 | +| **Vue 用 `{% raw %}` 包裹** | Jinja2 与 Vue 3 `{{}}` 冲突 | +| **单例 MissionExecutorV3** | 一个任务实例全局可见，方便停止 | +| **蛇形路径镜像只取反 J1** | 用户要求：同一边只需镜像 J1 关节 | +| **QR 弹窗不可跳过** | 业务约束：机型不在报关单/超量必须人工介入 | +| **上传序号全局递增** | 连续编号便于后端核对 | +| **环境切换无需重启** | 运行时动态修改 config 变量 + 代理 URL | + +### 10.2 已知约束 + +| 约束 | 影响 | +|------|------| +| **摄像头 device_index=4** | 固定的 Orbbec Gemini 设备号，不可修改 | +| **V4L2 设备独占** | 同时只能一个进程打开 /dev/video4 | +| **ROS 时钟漂移 ~5.5min** | 需检查 AGV 的 RTC/NTP 同步 | +| **机械臂精度 ±1.5°** | _wait_arm_ready 容差 2° 可能过严 | +| **AGV 重启自动切正式环境** | 无持久化配置方案 | +| **报关单数据依赖外部 API** | API 格式不稳定（裸数组 vs 包装对象） | + +--- + +## 附录 A：目录结构 + +``` +agv_app/ +├── app.py # Flask 主程序 (2132行) +├── config.py # 集中配置 +├── requirements.txt # Python 依赖 +├── templates/ +│ ├── index.html # AGV 控制页 +│ ├── setting.html # 设置页（网格/机型/报关单） +│ └── running.html # 任务运行页 +├── static/ +│ ├── css/style.css # 样式（深色主题） +│ └── js/ +│ ├── app.js # 控制页逻辑 +│ ├── setting.js # 设置页逻辑 +│ ├── running.js # 运行页逻辑 +│ └── vue3.global.prod.js # Vue 3 CDN +├── data/ +│ ├── mission_config.json # 网格尺寸+点位坐标 +│ ├── machines_config.json # 机器配置（正/背面） +│ ├── models_config.json # 机型配置（姿态组） +│ ├── qr_config.json # 二维码扫描姿态 +│ └── map_config.json # 地图配置 +├── utils/ +│ ├── mission_executor.py # 任务执行器 V3 (1198行) +│ ├── agv_controller_ros2.py # AGV 运动控制 (216行) +│ ├── arm_client.py # 机械臂客户端 (170行) +│ ├── nav2_navigator.py # Nav2 导航器 (350行) +│ ├── qr_scanner.py # 二维码扫描 (170行) +│ └── image_uploader.py # 图片上传 (76行) +``` + +启动脚本位于仓库顶层 `scripts/`。LiDAR 时间戳修复脚本部署在 AGV 的 +`/home/elephant/work/scan_fixer/`，由 `scripts/start_all.sh` 调用。 + +## 附录 B：关键依赖 + +``` +flask>=2.0 +flask-cors +pyyaml +opencv-python +numpy +requests +pyzbar # 二维码识别（优先引擎） +Pillow # pyzbar 依赖 +rclpy # ROS2 Python 客户端 +nav2_simple_commander # Nav2 Python API +geometry_msgs # ROS2 消息类型 +``` + +--- + +> **文档维护**: 本文档随代码同步更新。关键变更请记录到 `memory/YYYY-MM-DD.md`。 diff --git a/docs/开发记录_2026-05_06.md b/docs/开发记录_2026-05_06.md new file mode 100644 index 0000000..320580f --- /dev/null +++ b/docs/开发记录_2026-05_06.md @@ -0,0 +1,151 @@ +# AGV + 机械臂 移动拍摄平台 — 开发记录 + +> 汇总 2026年5-6月期间的所有修复记录和任务总结 + +--- + +## 一、running.html 显示修复 + 任务执行状态实时更新 (2026-05-29 13:10) + +### 目标 +修复运行页面两个 bug： +1. 模板中 `{{ }}` 显示为原始文本（Vue 未挂载） +2. 任务执行过程中状态不更新（始终显示"⏳等待"） + +### 根因分析 + +**问题1：`{{ }}` 原文显示** +- `running.js` 写有 `delimiters: ['[[', ']]']`，但 **Vue 3 已移除此选项**（被静默忽略） +- Vue 3 只认 `{{ }}`，但模板中混用了 `[[ ]]` 和 `{% raw %}{{ }}{% endraw %}` +- 残留的裸 `[[ ]]`（log、report、errorMsg 等）未被 Jinja2 处理，Vue 也因 delimiters 冲突不解析 +- **修复**：删除 `delimiters` 行 → 全部改用 `{% raw %}{{ }}{% endraw %}` 包裹 Vue 表达式 + +**问题2：状态不更新** +- `api_mission_state()` 每次都从文件初始化 `point_status`/`machine_status` 为全 `"pending"` +- `mission_executor.py` 完全没有跟踪 `point_status` 和 `machine_status` +- **修复**：executor 添加状态跟踪 + app.py 从 executor.report 读取实时状态 + +### 修改的文件 + +| 文件 | 改动 | +|------|------| +| `running.js` | 删除 `delimiters: ['[[', ']]']` | +| `running.html` | 全部 `[[ ]]` → `{% raw %}{{ }}{% endraw %}`（14处） | +| `app.py` | `api_mission_state()` 从 `ex.report` 读取 `point_status`/`machine_status` | +| `mission_executor.py` | 初始化+实时更新 `point_status`（pending/active/done/skipped）和 `machine_status`（pending/active/completed） | + +### 关键设计 + +**point_status 状态流转：** +- `pending` → `active`（开始导航到点位） → `done`（到达） → `skipped`（空位永不更新） + +**machine_status 状态流转：** +- 初始化全 `pending` +- 正面扫码开始：`status=active, step=正面扫码` +- 扫码完成：`qr=done/skipped, qr_val=xxx, step=正面拍照` +- 正面拍照完成：`front=done/skipped, front_cnt++` +- 背面拍照开始：`step=背面拍照` +- 背面拍照完成：`back=done/skipped, back_cnt++, status=completed, step=完成` + +### 部署状态 +- 所有4个文件已通过 scp 部署到 `192.168.50.93` +- Flask 已重启（PID 3664） +- API 验证通过：`point_status` 和 `machine_status` 正常返回 +- 本地文件已同步回 workspace + +--- + +## 二、AGV 蛇形路径关节反转逻辑 (2026-05-29 13:49) + +### 需求理解 + +蛇形路径行走时，AGV 在不同行到达点位时朝向相反： +- 偶数行(0,2,4...)点位 → AGV 从出发方向来 → 正面/背面朝向 = 标定朝向 → **不反转** +- 奇数行(1,3,5...)点位 → AGV 从对面来 → 正面/背面朝向 = 标定朝向的反面 → **反转所有关节角度** + +### 修复内容 + +修改 `mission_executor.py`： + +**1. `_shoot()` 新增 `machine_row` 参数** +```python +def _shoot(self, model, side, row, col, qr_value, machine_row=0): + invert = (machine_row % 2 == 1) # 奇数行=反转 + if invert: + angles = [-a for a in angles] # 6个关节全部取反 +``` +调用处传入 `machine_row`（正面=pr，背面=pr-1） + +**2. `_scan_qr_with_poses()` 新增 `machine_row` 参数** +```python +def _scan_qr_with_poses(self, qr_configs, machine_row=0): + invert = (machine_row % 2 == 1) + if invert: + angles = [-a for a in angles] # 二维码扫描时也反转 +``` + +**3. 调用处传递 `machine_row`** +- `_scan_qr_with_poses(qr_configs, machine_row=pr)` — 正面扫码 +- `_shoot(model, "front", ..., pr)` — 正面拍照 +- `_shoot(model, "back", ..., pr-1)` — 背面拍照 + +### 部署状态 +- Flask PID 20577，AGV IP 192.168.50.93 +- 已通过语法检查 ✅ 已部署 ✅ + +--- + +## 三、修复删除机器姿态 404 错误 (2026-05-29) + +### 问题描述 +删除机器姿态时出现 404 错误： +``` +/api/mission/poses/m_1778767289/pose_1778767312/undefined +``` +URL 末尾出现 `undefined`，说明 `poseId` 参数丢失。 + +### 根因分析 +JS 中存在两个同名方法 `deletePose`： +1. **机型姿态** (L457): `deletePose(modelId, poseId)` → 调用 `/api/models/...` +2. **机器姿态** (L776): `deletePose(machineId, side, poseId)` → 调用 `/api/mission/poses/...` + +Vue 方法重载机制导致参数错位，`poseId` 变成 `undefined`。 + +### 修复方案 +将机器姿态方法重命名为 `deleteMachinePose`，避免命名冲突。 + +### 修改文件 +- `static/js/setting.js` L776: `deletePose` → `deleteMachinePose` + +### 部署 +- setting.js 已部署到 AGV +- setting.html 已部署到 AGV（版本号更新） +- 浏览器需刷新缓存 (Ctrl+F5) + +### 待确认 +- 模板中是否有调用 `deleteMachinePose` 的地方需同步修改 + +--- + +## 四、技术说明文档生成 (2026-06-17) + +### 任务 +为 AGV + 机械臂移动拍摄平台项目生成详细的技术说明文档 + +### 产出 +- **文件**: `AGV_机械臂_技术说明文档.md` (888行, 39.5KB) +- **内容覆盖**: + 1. 项目概述（业务目标、核心能力、技术栈） + 2. 系统架构（架构图、核心文件清单） + 3. 硬件环境与网络拓扑（设备清单、参数） + 4. 核心模块详解（GlobalState、MissionExecutorV3、AGVController、ArmClient、Nav2Navigator、QRScanner、ImageUploader） + 5. 通信协议（Flask↔前端、ROS2、TCP Socket、Java后端） + 6. 完整API接口文档（11个分组、98个端点） + 7. 任务执行流程（生命周期、QR扫描流程、拍照流程、错误处理） + 8. 数据配置格式（4种JSON schema） + 9. 部署与运维（启动流程、部署命令、常见问题） + 10. 关键决策与约束（10项架构决策 + 6项已知约束） + +### 数据来源 +- 逐文件阅读了全部7个Python源文件（共~4312行代码） +- 读取了4个数据配置文件 +- 结合记忆条目中的经验教训和已知问题 diff --git a/restart_agv.sh b/restart_agv.sh deleted file mode 100644 index f694b36..0000000 --- a/restart_agv.sh +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/bash -cd /home/elephant/work/agv_app - -# 语法检查 -python3 -m py_compile app.py -if [ $? -ne 0 ]; then - echo "Syntax error!" - exit 1 -fi - -# 重启服务 -pkill -f "python.*app.py" 2>/dev/null -sleep 1 -nohup python3 app.py > app.log 2>&1 & -sleep 3 - -# 验证 -if ss -tlnp | grep 5000; then - echo "✓ 端口5000 正常" - # 测试机械臂单帧 - result=$(curl -s --max-time 5 http://127.0.0.1:5000/api/camera/arm_refresh | head -c 4) - echo -n "arm_refresh: " - if [ "$result" = "$(echo -en '\xff\xd8\xff\xe0')" ]; then - echo "JPEG OK ✓" - else - echo "返回: $(echo $result | xxd | head -1)" - fi -else - echo "✗ 启动失败" - tail -10 app.log -fi \ No newline at end of file diff --git a/scripts/README.md b/scripts/README.md new file mode 100644 index 0000000..9755e9e --- /dev/null +++ b/scripts/README.md @@ -0,0 +1,72 @@ +# AGV 智能巡检系统 — 脚本说明 + +## 目录结构 + +``` +scripts/ +├── start_all.sh ← 生产环境完整启动（ROS2 + Nav2 + Flask） +├── stop_all.sh ← 生产环境完整停止 +├── start_flask.sh ← 仅重启 Flask（修改代码后快速部署） +├── restart_flask.sh ← 语法检查 + 清缓存 + 重启 Flask + 验证 +└── dev_start.sh ← 本地开发用（前台运行，不启动 ROS2） +``` + +## 使用场景 + +### 1. 首次开机 / 完整重启 +```bash +# 在 AGV 上执行 +cd ~/work/smart-inspection/scripts +./stop_all.sh # 先彻底清理 +./start_all.sh # 完整启动 +``` + +### 2. 修改代码后快速部署 +```bash +# 部署文件到 AGV 后 +ssh elephant@192.168.60.80 'bash -s' < scripts/restart_flask.sh +``` + +### 3. 本地开发调试（不连硬件） +```bash +# 在本机执行，仅启动 Flask +./scripts/dev_start.sh +# 访问 http://127.0.0.1:5000 +``` + +### 4. 远程轻量重启（ROS2 已运行） +```bash +ssh elephant@192.168.60.80 'bash -s' < scripts/start_flask.sh +``` + +## 环境变量 + +所有脚本支持通过环境变量覆盖默认路径： + +| 变量 | 默认值 | 说明 | +|------|--------|------| +| `AGV_APP_DIR` | `/home/elephant/work/agv_app` | Flask 项目目录 | +| `AGV_ROS2_DIR` | `/home/elephant/agv_pro_ros2` | ROS2 工作空间 | +| `SCAN_FIXER_DIR` | `/home/elephant/work/scan_fixer` | 时间戳修正工具目录 | +| `FIXER_SCRIPT` | `fix_scan_timestamp_v6.py` | fixer 脚本名 | + +## 日志位置（AGV 上） + +| 组件 | 日志 | +|------|------| +| bringup (激光雷达) | `/tmp/ros2_bringup.log` | +| Nav2 导航 | `/tmp/ros2_nav2.log` | +| scan fixer | `/tmp/scan_fixer.log` | +| Flask | `/tmp/agv_flask.log` | + +## 机械臂端 + +机械臂 (Pi) 的启动由 systemd 托管，在 Pi 上执行： + +```bash +sudo systemctl start arm_server # 启动 +sudo systemctl status arm_server # 查看状态 +sudo systemctl enable arm_server # 开机自启 +``` + +配置见 `arm_server/arm_server.service`。 diff --git a/scripts/dev_start.sh b/scripts/dev_start.sh new file mode 100755 index 0000000..548ed20 --- /dev/null +++ b/scripts/dev_start.sh @@ -0,0 +1,39 @@ +#!/bin/bash +# ============================================================ +# dev_start.sh - 本地开发环境启动（不启动 ROS2/机械臂硬件） +# 用法: ./scripts/dev_start.sh +# ============================================================ +set -e + +SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)" +PROJECT_DIR="$(dirname "$SCRIPT_DIR")" +AGV_APP_DIR="$PROJECT_DIR/agv_app" + +echo "==========================================" +echo " 本地开发模式 - 仅启动 Flask" +echo "==========================================" +echo "" + +# 切换到项目目录 +cd "$AGV_APP_DIR" + +# 检查是否有运行的 Flask 进程 +FLASK_PID=$(pgrep -f "python.*app.py" 2>/dev/null || true) +if [ -n "$FLASK_PID" ]; then + echo "Flask 已在运行 (PID: $FLASK_PID)" + read -p "是否重启? [y/N] " -n 1 -r + echo + if [[ $REPLY =~ ^[Yy]$ ]]; then + kill "$FLASK_PID" 2>/dev/null + sleep 1 + else + echo "保持现有进程，退出" + exit 0 + fi +fi + +# 使用前台模式运行（方便看日志和 Ctrl+C 停止） +echo "启动 Flask (前台模式，Ctrl+C 停止)..." +echo "访问: http://127.0.0.1:5000" +echo "" +exec python3 app.py diff --git a/scripts/restart_flask.sh b/scripts/restart_flask.sh new file mode 100755 index 0000000..53b8110 --- /dev/null +++ b/scripts/restart_flask.sh @@ -0,0 +1,58 @@ +#!/bin/bash +# ============================================================ +# restart_flask.sh - 语法检查 + 重启 Flask + 验证 +# 用法: ssh elephant@ 'bash -s' < scripts/restart_flask.sh +# 或在 AGV 上: cd ~/work/agv_app && ../../scripts/restart_flask.sh +# ============================================================ +set -e + +AGV_APP_DIR="${AGV_APP_DIR:-/home/elephant/work/agv_app}" +cd "$AGV_APP_DIR" + +echo "==========================================" +echo " 重启 Flask 服务" +echo "==========================================" +echo "" + +# 1. 语法检查 +echo "[1/3] Python 语法检查..." +python3 -m py_compile app.py +if [ $? -ne 0 ]; then + echo "❌ 语法错误，请先修复" + exit 1 +fi +echo " ✅ 语法检查通过" + +# 2. 清缓存 + 重启 +echo "[2/3] 清理缓存并重启..." +find "$AGV_APP_DIR" -name '*.pyc' -delete 2>/dev/null +find "$AGV_APP_DIR" -name '__pycache__' -type d -exec rm -rf {} + 2>/dev/null + +pkill -f "python.*app.py" 2>/dev/null || true +sleep 1 +nohup python3 app.py > /tmp/agv_flask.log 2>&1 & +FLASK_PID=$! +echo " Flask PID: $FLASK_PID" + +# 3. 验证 +echo "[3/3] 验证服务..." +sleep 3 +if ss -tlnp 2>/dev/null | grep -q 5000 || netstat -tlnp 2>/dev/null | grep -q 5000; then + echo " ✅ 端口 5000 正常监听" + # 测试机械臂摄像头单帧 + result=$(curl -s --max-time 5 http://127.0.0.1:5000/api/camera/arm_refresh 2>/dev/null | head -c 4) + if [ "$result" = "$(echo -en '\xff\xd8\xff\xe0')" ]; then + echo " ✅ arm_refresh 返回 JPEG" + else + echo " ⚠️ arm_refresh 返回异常（机械臂可能未连接）" + fi +else + echo " ❌ 端口 5000 未监听，查看日志:" + tail -10 /tmp/agv_flask.log + exit 1 +fi + +echo "" +echo "==========================================" +echo " ✅ 重启完成" +echo "==========================================" diff --git a/agv_app/start_all.sh b/scripts/start_all.sh similarity index 95% rename from agv_app/start_all.sh rename to scripts/start_all.sh index f073e0a..a7c917a 100755 --- a/agv_app/start_all.sh +++ b/scripts/start_all.sh @@ -9,8 +9,11 @@ # ============================================================ set -e -AGV_APP_DIR="/home/elephant/work/agv_app" -AGV_ROS2_DIR="/home/elephant/agv_pro_ros2" +# ---- 可配置项（环境变量覆盖默认值） ---- +AGV_APP_DIR="${AGV_APP_DIR:-/home/elephant/work/agv_app}" +AGV_ROS2_DIR="${AGV_ROS2_DIR:-/home/elephant/agv_pro_ros2}" +SCAN_FIXER_DIR="${SCAN_FIXER_DIR:-/home/elephant/work/scan_fixer}" +FIXER_SCRIPT="${FIXER_SCRIPT:-fix_scan_timestamp_v6.py}" ROS_DOMAIN_ID_VAL=1 echo "==========================================" @@ -138,7 +141,7 @@ fi echo "[3.5/8] 启动系统时钟发布器 (clock_publisher)..." nohup bash -c "source /opt/ros/humble/setup.bash && \ - ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/clock_publisher.py" \ + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 $SCAN_FIXER_DIR/clock_publisher.py" \ > /tmp/clock_publisher.log 2>&1 & CLOCK_PID=$! echo " clock_publisher PID: $CLOCK_PID" @@ -170,7 +173,7 @@ if [ "$SCAN_OK" -eq 0 ]; then fi nohup bash -c "source /opt/ros/humble/setup.bash && \ - ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp_v6.py" \ + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 $SCAN_FIXER_DIR/$FIXER_SCRIPT" \ > /tmp/scan_fixer.log 2>&1 & FIXER_PID=$! echo " fix_scan_timestamp PID: $FIXER_PID" @@ -185,7 +188,7 @@ pkill -f "clock_publisher" 2>/dev/null || true sleep 2 rm -f /tmp/scan_fixer.lock nohup bash -c "source /opt/ros/humble/setup.bash && \ - ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp_v6.py" \ + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 $SCAN_FIXER_DIR/$FIXER_SCRIPT" \ > /tmp/scan_fixer.log 2>&1 & FIXER_PID=$! sleep 3 diff --git a/scripts/start_flask.sh b/scripts/start_flask.sh new file mode 100755 index 0000000..c3f729e --- /dev/null +++ b/scripts/start_flask.sh @@ -0,0 +1,20 @@ +#!/bin/bash +# ============================================================ +# start_flask.sh - 仅启动/重启 Flask 服务（不启动 ROS2） +# 适用于: 修改了前端/API 代码后快速重启 +# ============================================================ +AGV_APP_DIR="${AGV_APP_DIR:-/home/elephant/work/agv_app}" + +pkill -f "python.*app.py" 2>/dev/null || true +sleep 1 + +cd "$AGV_APP_DIR" +nohup python3 app.py > /tmp/agv_flask.log 2>&1 & +echo "Flask started, PID: $!" +sleep 2 + +if ss -tlnp 2>/dev/null | grep -q 5000 || netstat -tlnp 2>/dev/null | grep -q 5000; then + echo "✅ 端口 5000 正常" +else + echo "⚠️ 端口 5000 未监听，检查 /tmp/agv_flask.log" +fi diff --git a/agv_app/stop_all.sh b/scripts/stop_all.sh similarity index 100% rename from agv_app/stop_all.sh rename to scripts/stop_all.sh diff --git a/shared/config.json b/shared/config.json deleted file mode 100644 index 360daa4..0000000 --- a/shared/config.json +++ /dev/null @@ -1,23 +0,0 @@ -{ - "agv": { - "ip": "192.168.60.80", - "ssh_user": "elephant", - "ssh_password": "Elephant", - "map_file": "map.yaml", - "map_dir": "/home/elephant" - }, - "arm": { - "ip": "192.168.60.120", - "ssh_user": "pi", - "ssh_password": "elephant", - "socket_port": 5001, - "roboflow_host": "127.0.0.1", - "roboflow_port": 5001 - }, - "app": { - "upload_url": "https://ts.timeddd.com/prod-api/file/uploadImage", - "agv_control_port": 5000, - "arm_server_port": 5002, - "secret_key": "agv630_secret_key_2024" - } -} \ No newline at end of file diff --git a/start_agv.sh b/start_agv.sh deleted file mode 100644 index 7e825a4..0000000 --- a/start_agv.sh +++ /dev/null @@ -1,24 +0,0 @@ -#!/bin/bash -# AGV 服务启动脚本 -cd /home/elephant/work/agv_app - -# 确保没有旧进程 -pkill -f "python.*app.py" 2>/dev/null -sleep 1 - -# 启动服务 -nohup python3 app.py > app.log 2>&1 & -PID=$! -echo "Started PID=$PID" - -sleep 3 - -# 验证 -if ss -tlnp | grep 5000; then - echo "✓ 端口 5000 监听正常" - curl -s http://127.0.0.1:5000/api/mission/state - echo "" -else - echo "✗ 端口 5000 未监听，检查日志：" - cat app.log -fi \ No newline at end of file