任务执行
This commit is contained in:
ywb
Binary file not shown.
@@ -0,0 +1,650 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
任务执行器 V3 — M×N 网格蛇形路径拍摄
|
||||
|
||||
工作流：
|
||||
1. 根据 grid 生成蛇形路径（奇数行左→右，偶数行右→左）
|
||||
2. 逐台机器：
|
||||
- 正面：导航 → 扫码(多姿态重试) → 查机型 → 按姿态拍照
|
||||
- 背面：导航 → 按姿态拍照
|
||||
3. 回到 (0,0)
|
||||
"""
|
||||
import os
|
||||
import time
|
||||
import json
|
||||
import logging
|
||||
import threading
|
||||
import subprocess
|
||||
import math
|
||||
from typing import Optional, Dict, List
|
||||
from enum import Enum
|
||||
|
||||
import requests
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
|
||||
from config import ARM_CAMERA_CONFIG
|
||||
ARM_CAMERA_SNAPSHOT = ARM_CAMERA_CONFIG["snapshot_url"]
|
||||
PHOTOS_DIR = "/home/elephant/photos"
|
||||
|
||||
# 二维码扫描重试参数
|
||||
QR_SCAN_TIMEOUT = 5 # 单次扫描超时
|
||||
QR_POSE_WAIT = 1.5 # 调整姿态后等待时间
|
||||
MANUAL_QR_TIMEOUT = 300 # 5分钟超时
|
||||
|
||||
|
||||
class MissionStatus(str, Enum):
|
||||
IDLE = "idle"
|
||||
RUNNING = "running"
|
||||
PAUSED = "paused"
|
||||
COMPLETED = "completed"
|
||||
WAITING_QR = "waiting_qr"
|
||||
|
||||
|
||||
class MissionExecutorV3:
|
||||
"""任务执行器 V3 — M×N 网格蛇形路径"""
|
||||
|
||||
_instance = None # 单例，供外部停止用
|
||||
|
||||
def __init__(self, config: dict):
|
||||
self.config = config
|
||||
self.status = MissionStatus.IDLE
|
||||
MissionExecutorV3._instance = self
|
||||
|
||||
# 实时状态报告
|
||||
self.report = {
|
||||
"status": "idle",
|
||||
"step": "",
|
||||
"progress": 0,
|
||||
"total": 0,
|
||||
"log": [],
|
||||
"error": None,
|
||||
}
|
||||
|
||||
# 线程同步
|
||||
self._stop = threading.Event()
|
||||
self._pause = threading.Event()
|
||||
self._pause.set() # 初始不暂停
|
||||
self._qr_event = threading.Event()
|
||||
self._qr_value: Optional[str] = None
|
||||
|
||||
# 设备
|
||||
from .arm_client import ArmClient
|
||||
self.arm_client = ArmClient(
|
||||
config["arm"]["host"],
|
||||
config["arm"]["port"]
|
||||
)
|
||||
|
||||
from .agv_controller_ros2 import AGVController
|
||||
self.agv = AGVController(
|
||||
device=config.get("device", "/dev/agvpro_controller"),
|
||||
baudrate=config.get("baudrate", 1000000)
|
||||
)
|
||||
|
||||
# ==================== 连接 ====================
|
||||
|
||||
def connect_all(self) -> Dict[str, bool]:
|
||||
results = {}
|
||||
results["agv"] = self.agv.connect()
|
||||
results["arm"] = self.arm_client.connect()
|
||||
return results
|
||||
|
||||
def disconnect_all(self):
|
||||
if self.arm_client:
|
||||
self.arm_client.close()
|
||||
self.agv.disconnect()
|
||||
|
||||
# ==================== 主任务流程 ====================
|
||||
|
||||
def execute_mission(
|
||||
self,
|
||||
mission_config: dict,
|
||||
machines: list,
|
||||
qr_configs: list,
|
||||
models: list,
|
||||
) -> dict:
|
||||
"""
|
||||
执行完整拍摄任务。
|
||||
|
||||
Args:
|
||||
mission_config: {rows, cols, grid, positions}
|
||||
machines: [{id, row, col, front: {coords}, back: {coords}}]
|
||||
qr_configs: [{id, name, joint_angles: [a1..a6]}]
|
||||
models: [{id, name, poses: [{name, arm_angles}], poses_back: [...]}]
|
||||
|
||||
Returns:
|
||||
执行报告 dict
|
||||
"""
|
||||
self.status = MissionStatus.RUNNING
|
||||
self.report = {"status": "running", "step": "初始化", "progress": 0, "total": 0, "log": [], "error": None}
|
||||
self._stop.clear()
|
||||
self._pause.set()
|
||||
|
||||
start_time = time.time()
|
||||
|
||||
try:
|
||||
rows = int(mission_config.get("rows", 1))
|
||||
cols = int(mission_config.get("cols", 1))
|
||||
grid = mission_config.get("grid", [])
|
||||
positions = mission_config.get("positions", [])
|
||||
|
||||
# 1. 生成蛇形路径
|
||||
path = self._build_snake_path(rows, cols, grid)
|
||||
if not path:
|
||||
self._log("❌ 网格中没有机器，任务终止")
|
||||
self.report["error"] = "No machines in grid"
|
||||
return self._finish(0)
|
||||
|
||||
self.report["total"] = len(path)
|
||||
self._log(f"📍 蛇形路径生成: {len(path)} 台机器")
|
||||
|
||||
# 初始化任务列表
|
||||
self.report["tasks"] = [{
|
||||
"row": r, "col": c,
|
||||
"machine_id": f"m_{r}_{c}",
|
||||
"label": f"{r+1}-{c+1}",
|
||||
"status": "pending",
|
||||
"step": "等待",
|
||||
"qr_value": None,
|
||||
"photos_front": 0,
|
||||
"photos_back": 0,
|
||||
} for (r, c) in path]
|
||||
|
||||
# 初始化任务列表
|
||||
self.report["tasks"] = [{
|
||||
"row": r, "col": c,
|
||||
"machine_id": f"m_{r}_{c}",
|
||||
"label": f"{r+1}-{c+1}",
|
||||
"status": "pending",
|
||||
"step": "等待",
|
||||
"qr_value": None,
|
||||
"photos_front": 0,
|
||||
"photos_back": 0,
|
||||
} for (r, c) in path]
|
||||
|
||||
# 2. 逐台执行
|
||||
for idx, (r, c) in enumerate(path):
|
||||
if self._stop.is_set():
|
||||
self._log("⏹️ 任务已停止")
|
||||
break
|
||||
|
||||
self._wait_pause()
|
||||
|
||||
# 更新任务状态 → 正面开始
|
||||
task = self._get_task(r, c)
|
||||
if task:
|
||||
task["status"] = "active"
|
||||
task["step"] = "正面扫码"
|
||||
|
||||
machine_id = f"m_{r}_{c}"
|
||||
machine = next((m for m in machines if m.get("id") == machine_id), None)
|
||||
if not machine:
|
||||
self._log(f"⚠️ 机器 {r+1}-{c+1} 不存在，跳过")
|
||||
continue
|
||||
|
||||
rl, cl = r + 1, c + 1 # 显示用的 1-based
|
||||
|
||||
# --- 正面 ---
|
||||
self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
|
||||
self._step(f"机器 {rl}-{cl} 正面")
|
||||
|
||||
# 导航到正面点位
|
||||
front_pt = self._find_point(positions, r, c, "front")
|
||||
if front_pt and self._has_coords(front_pt):
|
||||
if not self._navigate(front_pt, "正面"):
|
||||
self._log(f"⚠️ 导航失败，尝试继续")
|
||||
else:
|
||||
self._log(f"⚠️ 无正面点位坐标")
|
||||
|
||||
# 扫描二维码
|
||||
qr_value = self._scan_qr_with_poses(qr_configs)
|
||||
if self._stop.is_set():
|
||||
break
|
||||
|
||||
# 查机型 + 更新任务步骤
|
||||
model_name = self._lookup_model(qr_value)
|
||||
self._log(f" 🏷️ 机型: {model_name}")
|
||||
if task and qr_value:
|
||||
task["qr_value"] = qr_value
|
||||
if task:
|
||||
task["step"] = "正面拍照"
|
||||
|
||||
# 正面拍照
|
||||
model = self._find_model(models, model_name)
|
||||
if model:
|
||||
self._shoot(model, "front", rl, cl, qr_value or "unknown")
|
||||
else:
|
||||
self._log(f" ⚠️ 未找到机型 {model_name}")
|
||||
|
||||
self._progress(idx, 1)
|
||||
|
||||
# --- 背面 ---
|
||||
if task:
|
||||
task["step"] = "背面拍照"
|
||||
self._log(f"📍 机器 {rl}-{cl} 进入背面点位")
|
||||
self._step(f"机器 {rl}-{cl} 背面")
|
||||
|
||||
# 导航到背面点位
|
||||
back_pt = self._find_point(positions, r + 1, c, "back")
|
||||
if back_pt and self._has_coords(back_pt):
|
||||
if not self._navigate(back_pt, "背面"):
|
||||
self._log(f"⚠️ 导航失败，尝试继续")
|
||||
else:
|
||||
self._log(f"⚠️ 无背面点位坐标")
|
||||
|
||||
# 背面拍照
|
||||
if model:
|
||||
self._shoot(model, "back", rl, cl, qr_value or "unknown")
|
||||
|
||||
# 标记任务完成
|
||||
if task:
|
||||
task["status"] = "completed"
|
||||
task["step"] = "完成"
|
||||
self._progress(idx, 2)
|
||||
|
||||
# 3. 回到出发点
|
||||
if not self._stop.is_set():
|
||||
self._step("返回出发点")
|
||||
self._log("→ 返回 (0, 0)")
|
||||
self._nav2_go_to_point(0, 0, 0, timeout_sec=60)
|
||||
|
||||
elapsed = time.time() - start_time
|
||||
return self._finish(elapsed)
|
||||
|
||||
except Exception as e:
|
||||
self._log(f"❌ 任务异常: {e}")
|
||||
logger.exception("execute_mission 崩溃")
|
||||
self.report["error"] = str(e)
|
||||
self.status = MissionStatus.IDLE
|
||||
self.report["status"] = "idle"
|
||||
return self.report
|
||||
|
||||
def _finish(self, elapsed: float) -> dict:
|
||||
if self._stop.is_set():
|
||||
self._step("已停止")
|
||||
else:
|
||||
self._step("完成")
|
||||
self._log(f"✅ 任务完成 ({elapsed:.0f}s)")
|
||||
self.report["progress"] = 100
|
||||
self.status = MissionStatus.IDLE
|
||||
self.report["status"] = "idle"
|
||||
return self.report
|
||||
|
||||
# ==================== 蛇形路径 ====================
|
||||
|
||||
def _build_snake_path(self, rows: int, cols: int, grid: list) -> list:
|
||||
"""奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左"""
|
||||
path = []
|
||||
for r in range(rows):
|
||||
if r % 2 == 0:
|
||||
for c in range(cols):
|
||||
if self._has_machine(grid, r, c):
|
||||
path.append((r, c))
|
||||
else:
|
||||
for c in range(cols - 1, -1, -1):
|
||||
if self._has_machine(grid, r, c):
|
||||
path.append((r, c))
|
||||
return path
|
||||
|
||||
@staticmethod
|
||||
def _has_machine(grid: list, r: int, c: int) -> bool:
|
||||
if not grid or r >= len(grid):
|
||||
return False
|
||||
row = grid[r]
|
||||
if isinstance(row, list):
|
||||
return c < len(row) and bool(row[c])
|
||||
return False
|
||||
|
||||
# ==================== 点位查找 ====================
|
||||
|
||||
@staticmethod
|
||||
def _find_point(positions: list, row: int, col: int, side: str) -> Optional[dict]:
|
||||
for p in positions:
|
||||
if p.get("row") == row and p.get("col") == col and p.get("side") == side:
|
||||
return p
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def _has_coords(point: dict) -> bool:
|
||||
coords = point.get("coords", [])
|
||||
return len(coords) >= 2 and (coords[0] != 0 or coords[1] != 0)
|
||||
|
||||
# ==================== 导航 ====================
|
||||
|
||||
def _navigate(self, point: dict, label: str) -> bool:
|
||||
coords = point["coords"]
|
||||
x, y = float(coords[0]), float(coords[1])
|
||||
yaw = float(coords[2]) if len(coords) >= 3 else 0.0
|
||||
self._log(f" 🧭 导航到{label}点位 ({x:.2f}, {y:.2f}, yaw={math.degrees(yaw):.0f}°)")
|
||||
return self._nav2_go_to_point(x, y, yaw)
|
||||
|
||||
# ==================== 二维码扫描 ====================
|
||||
|
||||
def _scan_qr_with_poses(self, qr_configs: list) -> Optional[str]:
|
||||
"""用二维码配置中的姿态依次尝试"""
|
||||
if not qr_configs:
|
||||
self._log(f" ⚠️ 无二维码配置")
|
||||
return self._request_manual_qr()
|
||||
|
||||
self._log(f" 🔍 尝试 {len(qr_configs)} 个二维码姿态...")
|
||||
for i, qc in enumerate(qr_configs):
|
||||
if self._stop.is_set():
|
||||
return None
|
||||
self._wait_pause()
|
||||
|
||||
angles = qc.get("joint_angles", [])
|
||||
if not angles or len(angles) < 6:
|
||||
continue
|
||||
|
||||
name = qc.get("name", f"姿态{i+1}")
|
||||
self._log(f" [{i+1}/{len(qr_configs)}] {name}")
|
||||
|
||||
# 调整机械臂
|
||||
if self.arm_client:
|
||||
self.arm_client.set_angles(angles, speed=500)
|
||||
time.sleep(QR_POSE_WAIT)
|
||||
|
||||
# 扫码
|
||||
qr = self._decode_qr_from_arm()
|
||||
if qr:
|
||||
self._log(f" ✅ 识别成功: {qr}")
|
||||
return qr
|
||||
|
||||
time.sleep(0.3)
|
||||
|
||||
self._log(f" ⚠️ 全部 {len(qr_configs)} 个姿态均未识别到二维码")
|
||||
return self._request_manual_qr()
|
||||
|
||||
def _decode_qr_from_arm(self) -> Optional[str]:
|
||||
"""从机械臂摄像头取一帧，识别二维码"""
|
||||
for attempt in range(3):
|
||||
try:
|
||||
resp = requests.get(ARM_CAMERA_SNAPSHOT, timeout=QR_SCAN_TIMEOUT)
|
||||
if resp.status_code != 200 or not resp.content:
|
||||
continue
|
||||
|
||||
arr = np.frombuffer(resp.content, dtype=np.uint8)
|
||||
frame = cv2.imdecode(arr, cv2.IMREAD_COLOR)
|
||||
if frame is None:
|
||||
continue
|
||||
|
||||
detector = cv2.QRCodeDetector()
|
||||
data, bbox, _ = detector.detectAndDecode(frame)
|
||||
if data and len(data) > 0:
|
||||
return data
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.5)
|
||||
return None
|
||||
|
||||
def _request_manual_qr(self) -> Optional[str]:
|
||||
"""暂停任务，等待手动输入"""
|
||||
self.status = MissionStatus.WAITING_QR
|
||||
self.report["status"] = "waiting_qr"
|
||||
self.report["step"] = "等待手动输入二维码"
|
||||
self._log(" ⌨️ 弹窗等待手动输入二维码...")
|
||||
|
||||
self._qr_event.clear()
|
||||
if self._qr_event.wait(timeout=MANUAL_QR_TIMEOUT):
|
||||
self.status = MissionStatus.RUNNING
|
||||
self.report["status"] = "running"
|
||||
self._log(f" ✏️ 手动输入: {self._qr_value}")
|
||||
return self._qr_value
|
||||
else:
|
||||
self.status = MissionStatus.RUNNING
|
||||
self.report["status"] = "running"
|
||||
self._log(f" ⚠️ 等待超时（{MANUAL_QR_TIMEOUT}s），跳过")
|
||||
return None
|
||||
|
||||
def set_manual_qr(self, value: str):
|
||||
self._qr_value = value.strip()
|
||||
self._qr_event.set()
|
||||
|
||||
# ==================== 机型查询 ====================
|
||||
|
||||
def _lookup_model(self, qr_value: Optional[str]) -> str:
|
||||
"""TODO: 后续通过 HTTP 接口查询机型"""
|
||||
return "机器1"
|
||||
|
||||
@staticmethod
|
||||
def _find_model(models: list, name: str) -> Optional[dict]:
|
||||
"""在机型列表中找到匹配的机型"""
|
||||
for m in models:
|
||||
if m.get("name") == name or m.get("id") == name:
|
||||
return m
|
||||
# fallback: 第一个机型
|
||||
return models[0] if models else None
|
||||
|
||||
# ==================== 姿态拍照 ====================
|
||||
|
||||
def _shoot(self, model: dict, side: str, row: int, col: int, qr_value: str):
|
||||
"""按机型配置的所有姿态依次拍照"""
|
||||
# 更新任务照片计数
|
||||
task = self._get_task(row - 1, col - 1)
|
||||
side_label = "正面" if side == "front" else "背面"
|
||||
poses = model.get("poses", []) if side == "front" else model.get("poses_back", [])
|
||||
if not poses:
|
||||
self._log(f" ⚠️ 机型无{side_label}姿态配置")
|
||||
return
|
||||
|
||||
self._log(f" 📷 {side_label}拍照 ({len(poses)} 个姿态)")
|
||||
for pi, pose in enumerate(poses):
|
||||
if self._stop.is_set():
|
||||
break
|
||||
self._wait_pause()
|
||||
|
||||
angles = pose.get("arm_angles", [])
|
||||
if not angles or len(angles) < 6:
|
||||
self._log(f" 跳过 {pose.get('name', f'姿态{pi+1}')}: 无效角度")
|
||||
continue
|
||||
|
||||
name = pose.get("name", f"{side_label}-{pi+1}")
|
||||
self._log(f" 🎯 {name}")
|
||||
|
||||
# 调整机械臂
|
||||
if self.arm_client:
|
||||
self.arm_client.set_angles(angles, speed=500)
|
||||
time.sleep(QR_POSE_WAIT)
|
||||
|
||||
# 拍照
|
||||
path = self._capture_arm_photo(row, col, side, pi + 1, qr_value)
|
||||
if path:
|
||||
self._log(f" 💾 {os.path.basename(path)}")
|
||||
|
||||
def _capture_arm_photo(self, row: int, col: int, side: str,
|
||||
pose_idx: int, qr_value: str) -> Optional[str]:
|
||||
"""从机械臂摄像头拍照存本地"""
|
||||
try:
|
||||
resp = requests.get(ARM_CAMERA_SNAPSHOT, timeout=10)
|
||||
if resp.status_code != 200 or not resp.content:
|
||||
logger.error("arm snapshot 请求失败")
|
||||
return None
|
||||
|
||||
os.makedirs(PHOTOS_DIR, exist_ok=True)
|
||||
ts = time.strftime("%Y%m%d_%H%M%S")
|
||||
fname = f"{ts}_r{row}c{col}_{side}_p{pose_idx}_{qr_value[:20]}.jpg"
|
||||
fpath = os.path.join(PHOTOS_DIR, fname)
|
||||
with open(fpath, "wb") as f:
|
||||
f.write(resp.content)
|
||||
return fpath
|
||||
except Exception as e:
|
||||
logger.error(f"拍照异常: {e}")
|
||||
return None
|
||||
|
||||
# ==================== 控制 ====================
|
||||
|
||||
def _wait_pause(self):
|
||||
"""等待暂停状态解除"""
|
||||
self._pause.wait()
|
||||
|
||||
def pause(self):
|
||||
self._pause.clear()
|
||||
self.status = MissionStatus.PAUSED
|
||||
self.report["status"] = "paused"
|
||||
self.report["step"] = "已暂停"
|
||||
self._log("⏸️ 任务已暂停")
|
||||
|
||||
def resume(self):
|
||||
self._pause.set()
|
||||
self.status = MissionStatus.RUNNING
|
||||
self.report["status"] = "running"
|
||||
self._log("▶️ 任务已恢复")
|
||||
|
||||
def stop(self):
|
||||
self._stop.set()
|
||||
self._pause.set() # 解除暂停
|
||||
self._qr_event.set() # 解除 QR 等待
|
||||
if self.arm_client:
|
||||
try:
|
||||
self.arm_client.task_stop()
|
||||
except Exception:
|
||||
pass
|
||||
self.agv.stop()
|
||||
self.status = MissionStatus.IDLE
|
||||
self.report["status"] = "idle"
|
||||
|
||||
def get_status(self) -> dict:
|
||||
return {
|
||||
"status": self.report["status"],
|
||||
"step": self.report["step"],
|
||||
"progress": self.report["progress"],
|
||||
"total": self.report["total"],
|
||||
}
|
||||
|
||||
def get_logs(self) -> dict:
|
||||
"""返回实时日志和完整状态"""
|
||||
return self.report
|
||||
|
||||
# ==================== 状态报告 ====================
|
||||
|
||||
def _log(self, msg: str):
|
||||
self.report["log"].append(msg)
|
||||
# Keep last 500 entries
|
||||
if len(self.report["log"]) > 500:
|
||||
self.report["log"] = self.report["log"][-500:]
|
||||
logger.info(msg)
|
||||
|
||||
def _step(self, text: str):
|
||||
self.report["step"] = text
|
||||
|
||||
def _get_task(self, row: int, col: int) -> Optional[dict]:
|
||||
"""获取指定行列的任务记录"""
|
||||
for t in self.report.get("tasks", []):
|
||||
if t["row"] == row and t["col"] == col:
|
||||
return t
|
||||
return None
|
||||
|
||||
def _progress(self, machine_idx: int, side_code: int):
|
||||
"""side_code: 1=正面完成, 2=背面完成"""
|
||||
if self.report["total"]:
|
||||
self.report["progress"] = min(
|
||||
int((machine_idx * 2 + side_code) / (self.report["total"] * 2) * 100),
|
||||
99
|
||||
)
|
||||
|
||||
# ==================== Nav2 导航 ====================
|
||||
# (保留原实现)
|
||||
|
||||
def _nav2_check_available(self) -> bool:
|
||||
try:
|
||||
rc, out, err = self._run_ros2_cmd("ros2 action list")
|
||||
if rc != 0:
|
||||
return False
|
||||
return "/navigate_to_pose" in out
|
||||
except:
|
||||
return False
|
||||
|
||||
def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
|
||||
timeout_sec: float = 120.0) -> bool:
|
||||
if not self._nav2_check_available():
|
||||
logger.error("Nav2 action server 不可用")
|
||||
return False
|
||||
|
||||
qz = math.sin(yaw / 2.0)
|
||||
qw = math.cos(yaw / 2.0)
|
||||
|
||||
pose_yaml = (
|
||||
f"pose:\n"
|
||||
f" header:\n"
|
||||
f" stamp:\n"
|
||||
f" sec: 0\n"
|
||||
f" nanosec: 0\n"
|
||||
f" frame_id: 'map'\n"
|
||||
f" pose:\n"
|
||||
f" position:\n"
|
||||
f" x: {x}\n"
|
||||
f" y: {y}\n"
|
||||
f" z: 0.0\n"
|
||||
f" orientation:\n"
|
||||
f" x: 0.0\n"
|
||||
f" y: 0.0\n"
|
||||
f" z: {qz}\n"
|
||||
f" w: {qw}"
|
||||
)
|
||||
|
||||
cmd = (
|
||||
f"ros2 action send_goal /navigate_to_pose "
|
||||
f"nav2_msgs/action/NavigateToPose "
|
||||
f"'{pose_yaml}' --feedback"
|
||||
)
|
||||
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
|
||||
|
||||
try:
|
||||
process = subprocess.Popen(
|
||||
full_cmd, shell=True,
|
||||
stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
|
||||
)
|
||||
succeeded = False
|
||||
elapsed = 0.0
|
||||
while elapsed < timeout_sec:
|
||||
import select
|
||||
reads = [process.stdout]
|
||||
ready, _, _ = select.select(reads, [], [], 1.0)
|
||||
elapsed += 1.0
|
||||
if process.stdout in ready:
|
||||
line = process.stdout.readline()
|
||||
if not line:
|
||||
break
|
||||
ls = line.strip()
|
||||
if "succeeded" in ls.lower():
|
||||
succeeded = True
|
||||
break
|
||||
elif "failed" in ls.lower() or "aborted" in ls.lower():
|
||||
break
|
||||
elif "canceled" in ls.lower() or "cancelled" in ls.lower():
|
||||
break
|
||||
if process.poll() is not None:
|
||||
break
|
||||
if process.poll() is None:
|
||||
process.terminate()
|
||||
try:
|
||||
process.wait(timeout=3)
|
||||
except subprocess.TimeoutExpired:
|
||||
process.kill()
|
||||
return succeeded
|
||||
except Exception as e:
|
||||
logger.error(f"Nav2 异常: {e}")
|
||||
return False
|
||||
|
||||
def _nav2_cancel(self):
|
||||
cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose 2>/dev/null || true'"
|
||||
try:
|
||||
subprocess.run(cancel_cmd, shell=True, timeout=3)
|
||||
except:
|
||||
pass
|
||||
|
||||
def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
|
||||
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
|
||||
try:
|
||||
result = subprocess.run(
|
||||
full_cmd, shell=True,
|
||||
capture_output=True, text=True, timeout=timeout
|
||||
)
|
||||
return result.returncode, result.stdout.strip(), result.stderr.strip()
|
||||
except subprocess.TimeoutExpired:
|
||||
return -1, "", "Timeout"
|
||||
except Exception as e:
|
||||
return -1, "", str(e)
|
||||
@@ -0,0 +1,122 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>运行监控 - AGV 拍摄系统</title>
|
||||
<link rel="stylesheet" href="/static/css/style.css">
|
||||
</head>
|
||||
<body>
|
||||
<div id="app">
|
||||
<header class="topbar">
|
||||
<div class="logo">▶️ 任务运行</div>
|
||||
<nav class="nav">
|
||||
<a href="/" class="nav-link">🏠 首页</a>
|
||||
<a href="/setting" class="nav-link">⚙️ 设置</a>
|
||||
<a href="/running" class="nav-link active">▶️ 运行</a>
|
||||
</nav>
|
||||
</header>
|
||||
|
||||
<main class="container">
|
||||
<!-- 状态概览 -->
|
||||
<section class="card">
|
||||
<div class="running-header">
|
||||
<div class="running-status" :class="missionState">
|
||||
<span class="pulse"></span>
|
||||
[[ missionStateText ]]
|
||||
</div>
|
||||
<div class="running-progress" v-if="missionState === 'running' || missionState === 'waiting_qr'">
|
||||
<span>进度 [[ Math.round(progress) ]]%</span>
|
||||
<div class="progress-bar">
|
||||
<div class="progress-fill" :style="{width: progress + '%'}"></div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="btn-row">
|
||||
<button class="btn btn-success btn-large" @click="startMission" :disabled="missionState !== 'idle'">
|
||||
▶️ 开始任务
|
||||
</button>
|
||||
<button class="btn btn-warning btn-large" @click="pauseMission" :disabled="missionState !== 'running'">
|
||||
⏸️ 暂停
|
||||
</button>
|
||||
<button class="btn btn-primary btn-large" @click="resumeMission" :disabled="missionState !== 'paused'">
|
||||
▶️ 继续
|
||||
</button>
|
||||
<button class="btn btn-error btn-large" @click="stopMission" :disabled="missionState === 'idle'">
|
||||
⏹️ 停止
|
||||
</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 任务清单 -->
|
||||
<section class="card" v-if="tasks.length > 0">
|
||||
<h2>📋 任务清单 ([[ tasks.length ]] 台机器)</h2>
|
||||
<div class="task-grid">
|
||||
<div v-for="task in tasks" :key="task.machine_id"
|
||||
class="task-cell" :class="'task-' + task.status"
|
||||
:title="task.step">
|
||||
<div class="task-pos">[[ task.label ]]</div>
|
||||
<div class="task-status-icon">
|
||||
<span v-if="task.status === 'pending'">⏳</span>
|
||||
<span v-else-if="task.status === 'active'" class="pulse-icon">🔄</span>
|
||||
<span v-else-if="task.status === 'completed'">✅</span>
|
||||
<span v-else>❓</span>
|
||||
</div>
|
||||
<div class="task-step-text">[[ task.step ]]</div>
|
||||
<div class="task-info">
|
||||
<div v-if="task.qr_value" class="task-qr">🏷 [[ task.qr_value.substring(0,8) ]]</div>
|
||||
<div class="task-photos" v-if="task.photos_front || task.photos_back">
|
||||
📷 [[ task.photos_front ]]正 [[ task.photos_back ]]背
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 实时日志 -->
|
||||
<section class="card" v-if="missionState === 'running' || missionState === 'waiting_qr'">
|
||||
<h2>📜 实时日志</h2>
|
||||
<div class="log-box" ref="logBox">
|
||||
<div v-for="(log, i) in logs" :key="i" class="log-line">[[ log ]]</div>
|
||||
<div v-if="logs.length === 0" class="log-empty">等待任务开始...</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 实时预览 -->
|
||||
<section class="card">
|
||||
<h2>📷 摄像头预览</h2>
|
||||
<div class="camera-full">
|
||||
<img :src="previewUrl" @error="onPreviewError">
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 任务报告 -->
|
||||
<section class="card" v-if="report">
|
||||
<h2>📊 任务报告</h2>
|
||||
<div class="report-summary">
|
||||
<div class="stat ok">✅ 完成: [[ report.completed ]]</div>
|
||||
<div class="stat error">❌ 失败: [[ report.failed ]]</div>
|
||||
<div class="stat">总计: [[ report.total_points ]]</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 手动输入二维码弹窗 -->
|
||||
<div class="modal-overlay" v-if="showQrModal">
|
||||
<div class="modal">
|
||||
<h3>⌨️ 手动输入二维码</h3>
|
||||
<p>所有姿态均未识别到二维码，请手动输入：</p>
|
||||
<input type="text" v-model="qrValue" placeholder="输入二维码内容" autofocus @keyup.enter="submitQr">
|
||||
<div class="modal-actions">
|
||||
<button class="btn btn-primary" @click="submitQr">确认</button>
|
||||
<button class="btn" @click="cancelQr">跳过</button>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
</main>
|
||||
</div>
|
||||
|
||||
<script src="/static/js/vue3.global.prod.js"></script>
|
||||
<script src="/static/js/running.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,130 @@
|
||||
const { createApp } = Vue
|
||||
const API = ''
|
||||
|
||||
createApp({
|
||||
delimiters: ['[[', ']]'],
|
||||
|
||||
data() {
|
||||
return {
|
||||
missionState: 'idle',
|
||||
progress: 0,
|
||||
tasks: [],
|
||||
report: null,
|
||||
previewUrl: API + '/api/camera/preview',
|
||||
polling: null,
|
||||
logs: [],
|
||||
showQrModal: false,
|
||||
qrValue: '',
|
||||
}
|
||||
},
|
||||
computed: {
|
||||
missionStateText() {
|
||||
const map = {
|
||||
idle: '空闲',
|
||||
running: '任务运行中',
|
||||
paused: '已暂停',
|
||||
completed: '已完成',
|
||||
waiting_qr: '等待输入二维码'
|
||||
}
|
||||
return map[this.missionState] || '未知'
|
||||
},
|
||||
},
|
||||
mounted() {
|
||||
this.poll()
|
||||
},
|
||||
beforeUnmount() {
|
||||
if (this.polling) clearInterval(this.polling)
|
||||
},
|
||||
methods: {
|
||||
poll() {
|
||||
this.refresh()
|
||||
this.pollLogs()
|
||||
this.polling = setInterval(() => {
|
||||
this.refresh()
|
||||
this.pollLogs()
|
||||
}, 2000)
|
||||
},
|
||||
async refresh() {
|
||||
try {
|
||||
const res = await fetch(API + '/api/mission/state')
|
||||
const data = await res.json()
|
||||
this.missionState = data.status || 'idle'
|
||||
this.progress = data.progress || 0
|
||||
if (data.tasks) this.tasks = data.tasks
|
||||
|
||||
// QR 弹窗
|
||||
if (this.missionState === 'waiting_qr' && !this.showQrModal) {
|
||||
this.showQrModal = true
|
||||
this.qrValue = ''
|
||||
}
|
||||
|
||||
// 完成后获取报告
|
||||
if (this.missionState === 'idle' && this.tasks.length > 0) {
|
||||
const reportRes = await fetch(API + '/api/mission/report')
|
||||
const reportData = await reportRes.json()
|
||||
this.report = reportData.report
|
||||
}
|
||||
} catch (e) {}
|
||||
},
|
||||
async pollLogs() {
|
||||
if (this.missionState !== 'running' && this.missionState !== 'waiting_qr') return
|
||||
try {
|
||||
const res = await fetch(API + '/api/mission/log')
|
||||
const data = await res.json()
|
||||
if (data.log) this.logs = data.log
|
||||
if (data.progress != null) this.progress = data.progress
|
||||
if (data.tasks) this.tasks = data.tasks
|
||||
// 自动滚到底
|
||||
this.$nextTick(() => {
|
||||
const box = this.$refs.logBox
|
||||
if (box) box.scrollTop = box.scrollHeight
|
||||
})
|
||||
} catch (e) {}
|
||||
},
|
||||
async startMission() {
|
||||
if (this.missionState !== 'idle') return
|
||||
this.logs = []
|
||||
this.progress = 0
|
||||
this.report = null
|
||||
this.showQrModal = false
|
||||
await fetch(API + '/api/mission/start', { method: 'POST' })
|
||||
this.missionState = 'running'
|
||||
},
|
||||
async pauseMission() {
|
||||
await fetch(API + '/api/mission/pause', { method: 'POST' })
|
||||
this.missionState = 'paused'
|
||||
},
|
||||
async resumeMission() {
|
||||
await fetch(API + '/api/mission/resume', { method: 'POST' })
|
||||
this.missionState = 'running'
|
||||
this.showQrModal = false
|
||||
},
|
||||
async stopMission() {
|
||||
await fetch(API + '/api/mission/stop', { method: 'POST' })
|
||||
this.missionState = 'idle'
|
||||
this.showQrModal = false
|
||||
},
|
||||
async submitQr() {
|
||||
const val = this.qrValue.trim()
|
||||
await fetch(API + '/api/mission/manual-qr', {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify({ qr: val || ' ' })
|
||||
})
|
||||
this.showQrModal = false
|
||||
this.qrValue = ''
|
||||
},
|
||||
cancelQr() {
|
||||
this.showQrModal = false
|
||||
this.qrValue = ''
|
||||
fetch(API + '/api/mission/manual-qr', {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify({ qr: 'SKIP' })
|
||||
})
|
||||
},
|
||||
onPreviewError(e) {
|
||||
e.target.style.display = 'none'
|
||||
}
|
||||
}
|
||||
}).mount('#app')
|
||||
@@ -0,0 +1,67 @@
|
||||
#!/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()
|
||||
@@ -0,0 +1,17 @@
|
||||
#!/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
|
||||
Executable
+17
@@ -0,0 +1,17 @@
|
||||
#!/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
|
||||
Executable
+177
@@ -0,0 +1,177 @@
|
||||
#!/bin/bash
|
||||
# ============================================================
|
||||
# Robot AGV 全量启动脚本 v2.6
|
||||
# 修复：
|
||||
# - 清理 scan_fixer lock 文件防残留
|
||||
# - Nav2 节点检测 grep -c 改为单行输出
|
||||
# - nohup 启动 Nav2 用 bash -c 包裹（确保 source 环境）
|
||||
# ============================================================
|
||||
set -e
|
||||
|
||||
AGV_APP_DIR="/home/elephant/work/agv_app"
|
||||
AGV_ROS2_DIR="/home/elephant/agv_pro_ros2"
|
||||
ROS_DOMAIN_ID_VAL=1
|
||||
|
||||
echo "=========================================="
|
||||
echo " Robot AGV 全量启动 v2.6"
|
||||
echo "=========================================="
|
||||
echo ""
|
||||
|
||||
# ---------- 1. 清理旧进程（不杀 ros2-daemon） ----------
|
||||
echo "[1/7] 清理旧进程..."
|
||||
pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true
|
||||
pkill -f "ros2 launch agv_pro_navigation2" 2>/dev/null || true
|
||||
pkill -f "agv_pro_node" 2>/dev/null || true
|
||||
pkill -f "lslidar_driver_node" 2>/dev/null || true
|
||||
pkill -f "fix_scan_timestamp" 2>/dev/null || true
|
||||
pkill -f "python.*app.py" 2>/dev/null || true
|
||||
sleep 4
|
||||
|
||||
# 清理 scan_fixer 锁文件（防残留 PID 导致启动失败）
|
||||
rm -f /tmp/scan_fixer.lock
|
||||
|
||||
echo " 清理完成"
|
||||
|
||||
# ---------- 2. 重启 ros2 daemon ----------
|
||||
echo "[2/7] 重启 ros2 daemon..."
|
||||
pkill -f "ros2-daemon" 2>/dev/null || true
|
||||
sleep 2
|
||||
nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
|
||||
sleep 5
|
||||
echo " ros2 daemon 已就绪"
|
||||
|
||||
# ---------- 3. 启动 bringup (含激光雷达) ----------
|
||||
echo "[3/7] 启动 AGV Bringup..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd "$AGV_ROS2_DIR"
|
||||
source install/setup.bash
|
||||
nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py \
|
||||
port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
|
||||
BRINGUP_PID=$!
|
||||
echo " bringup PID: $BRINGUP_PID"
|
||||
|
||||
echo " 等待 bringup 就绪..."
|
||||
for i in $(seq 1 20); do
|
||||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then
|
||||
echo " ✅ bringup 已就绪"
|
||||
break
|
||||
fi
|
||||
sleep 2
|
||||
done
|
||||
|
||||
# ---------- 4. 启动激光时间戳修正节点 ----------
|
||||
echo "[4/7] 启动激光时间戳修正节点..."
|
||||
pkill -f "fix_scan_timestamp" 2>/dev/null || true
|
||||
sleep 2
|
||||
|
||||
# 确保 /scan 存在
|
||||
for i in $(seq 1 10); do
|
||||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then
|
||||
echo " /scan 话题已上线"
|
||||
break
|
||||
fi
|
||||
sleep 2
|
||||
done
|
||||
|
||||
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.py" \
|
||||
> /tmp/scan_fixer.log 2>&1 &
|
||||
FIXER_PID=$!
|
||||
echo " fix_scan_timestamp PID: $FIXER_PID"
|
||||
sleep 5
|
||||
|
||||
# 验证 fixer 进程和 scan_corrected
|
||||
FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0)
|
||||
if [ "$FIXER_COUNT" -gt 1 ]; then
|
||||
echo " ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..."
|
||||
pkill -f "fix_scan_timestamp" 2>/dev/null || true
|
||||
sleep 2
|
||||
rm -f /tmp/scan_fixer.lock
|
||||
nohup bash -c "source /opt/ros/humble/setup.bash && \
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
|
||||
> /tmp/scan_fixer.log 2>&1 &
|
||||
FIXER_PID=$!
|
||||
sleep 3
|
||||
fi
|
||||
|
||||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan_corrected'; then
|
||||
echo " ✅ /scan_corrected 已上线"
|
||||
else
|
||||
echo " ⚠️ /scan_corrected 未上线，检查日志："
|
||||
tail -5 /tmp/scan_fixer.log
|
||||
fi
|
||||
|
||||
# ---------- 5. 启动 Nav2 ----------
|
||||
echo "[5/7] 启动 Nav2 导航..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd "$AGV_ROS2_DIR"
|
||||
source install/setup.bash
|
||||
# 使用 bash -c 确保 source 环境变量传递到 nohup
|
||||
nohup bash -c "source /opt/ros/humble/setup.bash && \
|
||||
source /home/elephant/agv_pro_ros2/install/setup.bash && \
|
||||
export ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL && \
|
||||
ros2 launch agv_pro_navigation2 navigation2_active.launch.py \
|
||||
autostart:=True" > /tmp/ros2_nav2.log 2>&1 &
|
||||
NAV2_PID=$!
|
||||
echo " Nav2 PID: $NAV2_PID"
|
||||
sleep 12
|
||||
|
||||
echo " 等待 Nav2 节点就绪..."
|
||||
for i in $(seq 1 20); do
|
||||
NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
|
||||
grep -cE 'lifecycle_manager_navigation|bt_navigator|controller_server' 2>/dev/null || echo 0)
|
||||
# 去除可能的换行符，确保是单个数字
|
||||
NODES=$(echo "$NODES" | tr -d '\n' | awk '{print $1}')
|
||||
if [ "$NODES" -ge 3 ] 2>/dev/null; then
|
||||
echo " ✅ Nav2 节点已就绪 ($NODES 个)"
|
||||
break
|
||||
fi
|
||||
sleep 3
|
||||
done
|
||||
|
||||
# ---------- 6. 设置精度参数 ----------
|
||||
echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd "$AGV_ROS2_DIR"
|
||||
source install/setup.bash
|
||||
|
||||
for NODE in /controller_server /bt_navigator /planner_server; do
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
|
||||
done
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true
|
||||
echo " 精度参数已设置"
|
||||
|
||||
# ---------- 7. 启动 Flask ----------
|
||||
echo "[7/7] 启动 Flask API..."
|
||||
cd "$AGV_APP_DIR"
|
||||
nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
|
||||
FLASK_PID=$!
|
||||
echo " Flask PID: $FLASK_PID"
|
||||
sleep 4
|
||||
|
||||
# ---------- 完成 ----------
|
||||
echo ""
|
||||
echo "=========================================="
|
||||
echo " ✅ 启动完成"
|
||||
echo "=========================================="
|
||||
echo ""
|
||||
echo " 进程状态:"
|
||||
for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
|
||||
NAME="${PROC%%:*}"
|
||||
PID="${PROC##*:}"
|
||||
echo " $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
|
||||
done
|
||||
echo ""
|
||||
echo " 日志文件:"
|
||||
echo " bringup : /tmp/ros2_bringup.log"
|
||||
echo " Nav2 : /tmp/ros2_nav2.log"
|
||||
echo " fixer : /tmp/scan_fixer.log"
|
||||
echo " Flask : /tmp/agv_flask.log"
|
||||
echo ""
|
||||
echo " 关键验证命令:"
|
||||
echo " curl http://localhost:5000/api/navigate/status"
|
||||
echo " ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once"
|
||||
echo " ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once"
|
||||
Executable
+165
@@ -0,0 +1,165 @@
|
||||
#!/bin/bash
|
||||
# ============================================================
|
||||
# Robot AGV 全量启动脚本 v2.2
|
||||
# 完整流程：
|
||||
# 清理旧进程（不杀 daemon） -> 启动 bringup ->
|
||||
# 启动激光时间戳修正节点 -> 启动 Nav2 ->
|
||||
# 设置导航精度参数 -> 启动 Flask
|
||||
# ============================================================
|
||||
set -e
|
||||
|
||||
AGV_APP_DIR="/home/elephant/work/agv_app"
|
||||
AGV_ROS2_DIR="/home/elephant/agv_pro_ros2"
|
||||
ROS_DOMAIN_ID_VAL=1
|
||||
|
||||
echo "=========================================="
|
||||
echo " Robot AGV 全量启动 v2.2"
|
||||
echo "=========================================="
|
||||
echo ""
|
||||
|
||||
# ---------- 1. 清理旧进程（不杀 ros2-daemon） ----------
|
||||
echo "[1/7] 清理旧进程..."
|
||||
pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true
|
||||
pkill -f "ros2 launch agv_pro_navigation2" 2>/dev/null || true
|
||||
pkill -f "agv_pro_node" 2>/dev/null || true
|
||||
pkill -f "lslidar_driver_node" 2>/dev/null || true
|
||||
pkill -f "scan_timestamp_fixer" 2>/dev/null || true
|
||||
pkill -f "python.*app.py" 2>/dev/null || true
|
||||
sleep 4
|
||||
echo " 清理完成"
|
||||
|
||||
# ---------- 2. 重启 ros2 daemon（仅杀 daemon进程本身，不杀整个环境） ----------
|
||||
echo "[2/7] 重启 ros2 daemon..."
|
||||
pkill -f "ros2-daemon" 2>/dev/null || true
|
||||
sleep 2
|
||||
nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
|
||||
sleep 5
|
||||
echo " ros2 daemon 已就绪"
|
||||
|
||||
# ---------- 3. 启动 bringup (含激光雷达) ----------
|
||||
echo "[3/7] 启动 AGV Bringup..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd "$AGV_ROS2_DIR"
|
||||
source install/setup.bash
|
||||
nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py \
|
||||
port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
|
||||
BRINGUP_PID=$!
|
||||
echo " bringup PID: $BRINGUP_PID"
|
||||
|
||||
echo " 等待 bringup 就绪..."
|
||||
for i in $(seq 1 20); do
|
||||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then
|
||||
echo " ✅ bringup 已就绪"
|
||||
break
|
||||
fi
|
||||
sleep 2
|
||||
done
|
||||
|
||||
# ---------- 4. 启动激光时间戳修正节点（单例，不重复启动） ----------
|
||||
echo "[4/7] 启动激光时间戳修正节点..."
|
||||
# 确保只有1个 fixer 进程在运行
|
||||
pkill -f "scan_timestamp_fixer" 2>/dev/null || true
|
||||
sleep 2
|
||||
|
||||
for i in $(seq 1 10); do
|
||||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then
|
||||
echo " /scan 话题已上线"
|
||||
break
|
||||
fi
|
||||
sleep 2
|
||||
done
|
||||
|
||||
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.py" \
|
||||
> /tmp/scan_fixer.log 2>&1 &
|
||||
FIXER_PID=$!
|
||||
echo " scan_timestamp_fixer PID: $FIXER_PID"
|
||||
sleep 5
|
||||
|
||||
# 验证只有1个 fixer 进程
|
||||
FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0)
|
||||
if [ "$FIXER_COUNT" -gt 1 ]; then
|
||||
echo " ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..."
|
||||
pkill -f "scan_timestamp_fixer" 2>/dev/null || true
|
||||
sleep 2
|
||||
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.py" \
|
||||
> /tmp/scan_fixer.log 2>&1 &
|
||||
sleep 3
|
||||
fi
|
||||
|
||||
if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan_corrected'; then
|
||||
echo " ✅ /scan_corrected 已上线"
|
||||
else
|
||||
echo " ⚠️ /scan_corrected 未上线，检查日志："
|
||||
cat /tmp/scan_fixer.log
|
||||
fi
|
||||
|
||||
# ---------- 5. 启动 Nav2 ----------
|
||||
echo "[5/7] 启动 Nav2 导航..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd "$AGV_ROS2_DIR"
|
||||
source install/setup.bash
|
||||
nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py \
|
||||
autostart:=True > /tmp/ros2_nav2.log 2>&1 &
|
||||
NAV2_PID=$!
|
||||
echo " Nav2 PID: $NAV2_PID"
|
||||
sleep 12
|
||||
|
||||
echo " 等待 Nav2 节点就绪..."
|
||||
for i in $(seq 1 15); do
|
||||
NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
|
||||
grep -c "lifecycle_manager_navigation\|bt_navigator\|controller_server" 2>/dev/null || echo 0)
|
||||
if [ "$NODES" -ge 3 ]; then
|
||||
echo " ✅ Nav2 节点已就绪 ($NODES 个)"
|
||||
break
|
||||
fi
|
||||
sleep 3
|
||||
done
|
||||
|
||||
# ---------- 6. 设置精度参数 ----------
|
||||
echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
cd "$AGV_ROS2_DIR"
|
||||
source install/setup.bash
|
||||
|
||||
for NODE in /controller_server /bt_navigator /planner_server; do
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
|
||||
done
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
|
||||
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true
|
||||
echo " 精度参数已设置"
|
||||
|
||||
# ---------- 7. 启动 Flask ----------
|
||||
echo "[7/7] 启动 Flask API..."
|
||||
cd "$AGV_APP_DIR"
|
||||
nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
|
||||
FLASK_PID=$!
|
||||
echo " Flask PID: $FLASK_PID"
|
||||
sleep 4
|
||||
|
||||
# ---------- 完成 ----------
|
||||
echo ""
|
||||
echo "=========================================="
|
||||
echo " ✅ 启动完成"
|
||||
echo "=========================================="
|
||||
echo ""
|
||||
echo " 进程状态:"
|
||||
for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
|
||||
NAME="${PROC%%:*}"
|
||||
PID="${PROC##*:}"
|
||||
echo " $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
|
||||
done
|
||||
echo ""
|
||||
echo " 日志文件:"
|
||||
echo " bringup : /tmp/ros2_bringup.log"
|
||||
echo " Nav2 : /tmp/ros2_nav2.log"
|
||||
echo " fixer : /tmp/scan_fixer.log"
|
||||
echo " Flask : /tmp/agv_flask.log"
|
||||
echo ""
|
||||
echo " 关键验证命令:"
|
||||
echo " curl http://localhost:5000/api/navigate/status"
|
||||
echo " ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once"
|
||||
echo " ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once"
|
||||
Reference in New Issue
Block a user