Update project structure
This commit is contained in:
+15
@@ -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
|
||||||
@@ -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": ""
|
||||||
|
}
|
||||||
|
]
|
||||||
@@ -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()
|
|
||||||
@@ -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
|
|
||||||
@@ -1,5 +0,0 @@
|
|||||||
#!/bin/bash
|
|
||||||
# 启动 AGV 拍摄系统
|
|
||||||
|
|
||||||
cd ~/work/agv_app
|
|
||||||
python3 app.py
|
|
||||||
@@ -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
|
|
||||||
@@ -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: $!"
|
|
||||||
-1045
File diff suppressed because it is too large
Load Diff
@@ -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()
|
|
||||||
@@ -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"
|
|
||||||
@@ -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
|
|
||||||
@@ -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()
|
||||||
@@ -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
|
||||||
@@ -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 <JWT Token>`
|
||||||
|
- **重试**: 最多 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 <token>` 头。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 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/<id>` | PUT | 更新机器配置 | |
|
||||||
|
| `/api/mission/machines/<id>` | DELETE | 删除机器配置 | |
|
||||||
|
| `/api/mission/poses/<id>/<side>` | GET | 获取机器指定侧姿态 | - |
|
||||||
|
| `/api/mission/poses/<id>/<side>` | POST | 添加姿态到机器 | `{"arm_angles":[],"speed":500}` |
|
||||||
|
| `/api/mission/poses/<id>/<side>/<pid>` | DELETE | 删除姿态 | - |
|
||||||
|
| `/api/mission/qr_scan/<id>` | 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/<id>` | POST | 更新机型 | - |
|
||||||
|
| `/api/models/<id>` | DELETE | 删除机型 | - |
|
||||||
|
| `/api/models/poses/add` | POST | 添加姿态到机型 | `{"model_id":"xxx","name":"正1","photo_type":"front","arm_angles":[]}` |
|
||||||
|
| `/api/models/<id>/poses` | GET | 获取机型姿态列表 | - |
|
||||||
|
| `/api/models/<id>/poses/<pid>` | PUT | 更新姿态 | - |
|
||||||
|
| `/api/models/<id>/poses/<pid>` | DELETE | 删除姿态 | - |
|
||||||
|
|
||||||
|
### 6.9 二维码配置 API
|
||||||
|
|
||||||
|
| 路由 | 方法 | 说明 | 参数 |
|
||||||
|
|------|------|------|------|
|
||||||
|
| `/api/qr/configs` | GET | 获取所有二维码配置 | - |
|
||||||
|
| `/api/qr/configs` | POST | 添加二维码配置 | `{"name":"二维码1","joint_angles":[]}` |
|
||||||
|
| `/api/qr/configs/<id>` | PUT | 更新二维码配置 | - |
|
||||||
|
| `/api/qr/configs/<id>` | DELETE | 删除二维码配置 | - |
|
||||||
|
| `/api/qr/configs/<id>/read-angles` | POST | 读取当前臂角度写入配置 | - |
|
||||||
|
| `/api/qr/scan/<id>` | 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<rows 且 (pr,c) 有机器）
|
||||||
|
├── 切换到 QR 扫描姿态
|
||||||
|
├── _scan_qr_with_poses(qr_configs):
|
||||||
|
│ ├── 逐姿态尝试扫描（pyzbar + OpenCV）
|
||||||
|
│ ├── 失败 → 弹窗 _request_manual_qr()
|
||||||
|
│ └── 机型不在报关单 → 弹窗重新输入（不可跳过）
|
||||||
|
│
|
||||||
|
├── _lookup_model(qr_value):
|
||||||
|
│ ├── 请求 /api/customs/printer?serialNumber=xxx
|
||||||
|
│ ├── 超量检查（inspected >= 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`。
|
||||||
@@ -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个数据配置文件
|
||||||
|
- 结合记忆条目中的经验教训和已知问题
|
||||||
@@ -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
|
|
||||||
@@ -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`。
|
||||||
Executable
+39
@@ -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
|
||||||
Executable
+58
@@ -0,0 +1,58 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
# ============================================================
|
||||||
|
# restart_flask.sh - 语法检查 + 重启 Flask + 验证
|
||||||
|
# 用法: ssh elephant@<AGV_IP> '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 "=========================================="
|
||||||
@@ -9,8 +9,11 @@
|
|||||||
# ============================================================
|
# ============================================================
|
||||||
set -e
|
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
|
ROS_DOMAIN_ID_VAL=1
|
||||||
|
|
||||||
echo "=========================================="
|
echo "=========================================="
|
||||||
@@ -138,7 +141,7 @@ fi
|
|||||||
echo "[3.5/8] 启动系统时钟发布器 (clock_publisher)..."
|
echo "[3.5/8] 启动系统时钟发布器 (clock_publisher)..."
|
||||||
|
|
||||||
nohup bash -c "source /opt/ros/humble/setup.bash && \
|
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 &
|
> /tmp/clock_publisher.log 2>&1 &
|
||||||
CLOCK_PID=$!
|
CLOCK_PID=$!
|
||||||
echo " clock_publisher PID: $CLOCK_PID"
|
echo " clock_publisher PID: $CLOCK_PID"
|
||||||
@@ -170,7 +173,7 @@ if [ "$SCAN_OK" -eq 0 ]; then
|
|||||||
fi
|
fi
|
||||||
|
|
||||||
nohup bash -c "source /opt/ros/humble/setup.bash && \
|
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 &
|
> /tmp/scan_fixer.log 2>&1 &
|
||||||
FIXER_PID=$!
|
FIXER_PID=$!
|
||||||
echo " fix_scan_timestamp PID: $FIXER_PID"
|
echo " fix_scan_timestamp PID: $FIXER_PID"
|
||||||
@@ -185,7 +188,7 @@ pkill -f "clock_publisher" 2>/dev/null || true
|
|||||||
sleep 2
|
sleep 2
|
||||||
rm -f /tmp/scan_fixer.lock
|
rm -f /tmp/scan_fixer.lock
|
||||||
nohup bash -c "source /opt/ros/humble/setup.bash && \
|
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 &
|
> /tmp/scan_fixer.log 2>&1 &
|
||||||
FIXER_PID=$!
|
FIXER_PID=$!
|
||||||
sleep 3
|
sleep 3
|
||||||
Executable
+20
@@ -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
|
||||||
@@ -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"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -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
|
|
||||||
Reference in New Issue
Block a user