ywb
2bb8dff4e2
- 新增 agv_app/utils/nav2_navigator.py：Nav2Navigator 类，通过 ros2 action /navigate_to_pose 和 /navigate_through_poses 与 Nav2 通信，支持路径点导航 - app.py：navigate/to, navigate/path, navigate/status 三个 API 改用 Nav2Navigator - mission_executor.py：_execute_point 中调用 _nav2_go_to_point() 替代原来的 time.sleep 模拟移动，Nav2 负责从当前点到目标点的自主导航 - 原有的 map_navigator.py（A* + Pure Pursuit 自实现）保留不动，供无 Nav2 时降级用
385 lines
13 KiB
Python
385 lines
13 KiB
Python
"""
|
||
任务调度器 - 管理拍摄任务的执行
|
||
"""
|
||
import os
|
||
import json
|
||
import time
|
||
import logging
|
||
import subprocess
|
||
import math
|
||
from typing import List, Dict, Optional
|
||
from enum import Enum
|
||
|
||
from .arm_client import ArmClient
|
||
from .agv_controller_ros2 import AGVController
|
||
from .qr_scanner import QRScanner
|
||
from .image_uploader import ImageUploader
|
||
|
||
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 TaskStatus(Enum):
|
||
PENDING = "pending"
|
||
RUNNING = "running"
|
||
COMPLETED = "completed"
|
||
FAILED = "failed"
|
||
PAUSED = "paused"
|
||
|
||
|
||
class MissionExecutor:
|
||
"""任务执行器 - 负责按顺序执行点位拍摄任务"""
|
||
|
||
def __init__(self, config: dict):
|
||
self.config = config
|
||
self.status = TaskStatus.PENDING
|
||
self.current_point_index = 0
|
||
self.current_pose_index = 0
|
||
self.snapshot_serial_map = {} # {point_id: serial_number} 缓存已扫描的 serialNumber
|
||
|
||
# 初始化各模块
|
||
self.agv = AGVController(
|
||
device=config.get("device", "/dev/agvpro_controller"),
|
||
baudrate=config.get("baudrate", 1000000)
|
||
)
|
||
self.arm_client: Optional[ArmClient] = None
|
||
self.uploader = ImageUploader(
|
||
upload_url=config["upload_url"],
|
||
timeout=config.get("upload_timeout", 30),
|
||
max_retries=config.get("upload_retries", 3)
|
||
)
|
||
self.qr_scanner = QRScanner(device_index=config.get("camera_index", 0))
|
||
|
||
# ========== 连接管理 ==========
|
||
|
||
def connect_all(self) -> Dict[str, bool]:
|
||
"""连接 AGV、机械臂、摄像头"""
|
||
results = {}
|
||
|
||
# 连接 AGV
|
||
results["agv"] = self.agv.connect()
|
||
|
||
# 连接机械臂（通过 TCP）
|
||
arm_cfg = self.config["arm"]
|
||
self.arm_client = ArmClient(arm_cfg["host"], arm_cfg["port"])
|
||
results["arm"] = self.arm_client.connect()
|
||
|
||
# 打开摄像头
|
||
results["camera"] = self.qr_scanner.open()
|
||
|
||
return results
|
||
|
||
def disconnect_all(self):
|
||
"""断开所有连接"""
|
||
if self.arm_client:
|
||
self.arm_client.close()
|
||
self.agv.disconnect()
|
||
self.qr_scanner.close()
|
||
|
||
# ========== 任务执行 ==========
|
||
|
||
def execute_mission(self, mission_data: dict) -> dict:
|
||
"""
|
||
执行一个完整任务（一个地图的所有点位）
|
||
mission_data: 包含点位列表的完整任务配置
|
||
返回执行报告
|
||
"""
|
||
self.status = TaskStatus.RUNNING
|
||
report = {
|
||
"total_points": len(mission_data.get("points", [])),
|
||
"completed": 0,
|
||
"failed": 0,
|
||
"details": []
|
||
}
|
||
|
||
points = mission_data.get("points", [])
|
||
for i, point in enumerate(points):
|
||
self.current_point_index = i
|
||
try:
|
||
result = self._execute_point(point)
|
||
report["details"].append(result)
|
||
if result["status"] == "completed":
|
||
report["completed"] += 1
|
||
else:
|
||
report["failed"] += 1
|
||
except Exception as e:
|
||
logger.error(f"点位 {i} 执行异常: {e}")
|
||
report["failed"] += 1
|
||
report["details"].append({
|
||
"point_index": i,
|
||
"point_name": point.get("name", f"point_{i}"),
|
||
"status": "failed",
|
||
"error": str(e)
|
||
})
|
||
|
||
self.status = TaskStatus.COMPLETED if report["failed"] == 0 else TaskStatus.PAUSED
|
||
return report
|
||
|
||
def _execute_point(self, point: dict) -> dict:
|
||
"""执行单个点位的拍摄"""
|
||
point_name = point.get("name", "unknown")
|
||
logger.info(f"开始执行点位: {point_name}")
|
||
|
||
result = {
|
||
"point_name": point_name,
|
||
"poses": []
|
||
}
|
||
|
||
# 1. AGV 移动到点位（Nav2 导航）
|
||
coords = point.get("coords", {})
|
||
x, y = coords.get("x", 0), coords.get("y", 0)
|
||
rz = coords.get("rz", 0.0) # 目标朝向
|
||
logger.info(f"AGV Nav2 导航到 ({x}, {y}), yaw={rz}")
|
||
|
||
nav_result = self._nav2_go_to_point(x, y, rz)
|
||
if not nav_result:
|
||
logger.warning(f"AGV 导航到点位 {point_name} 失败，跳过")
|
||
return {"point_name": point_name, "status": "failed", "error": "导航失败"}
|
||
|
||
# 2. 执行该点位的所有姿态
|
||
poses = point.get("poses", [])
|
||
for j, pose in enumerate(poses):
|
||
self.current_pose_index = j
|
||
pose_result = self._execute_pose(point, pose, j)
|
||
result["poses"].append(pose_result)
|
||
|
||
# 如果是"两者都要"类型，需要按顺序执行两台机器
|
||
if pose.get("type") == "both":
|
||
# 执行顺序由 pose.sequence 配置
|
||
sequence = pose.get("sequence", ["front_first"])
|
||
for step in sequence:
|
||
if step == "front":
|
||
self._capture_and_upload(point, pose, "front", j)
|
||
elif step == "back":
|
||
self._capture_and_upload(point, pose, "back", j)
|
||
else:
|
||
photo_type = pose.get("photo_type", "front")
|
||
self._capture_and_upload(point, pose, photo_type, j)
|
||
|
||
result["status"] = "completed"
|
||
return result
|
||
|
||
def _execute_pose(self, point: dict, pose: dict, pose_idx: int) -> dict:
|
||
"""执行单个姿态的拍摄"""
|
||
photo_type = pose.get("photo_type", "front")
|
||
camera_source = pose.get("camera", "agv") # agv 或 arm
|
||
|
||
# 如果需要机械臂运动
|
||
arm_angles = pose.get("arm_angles", None)
|
||
if arm_angles and self.arm_client:
|
||
self.arm_client.set_angles(arm_angles, speed=pose.get("speed", 500))
|
||
time.sleep(1) # 等待运动到位
|
||
|
||
return {
|
||
"pose_index": pose_idx,
|
||
"photo_type": photo_type,
|
||
"arm_angles": arm_angles,
|
||
"status": "ready"
|
||
}
|
||
|
||
def _capture_and_upload(self, point: dict, pose: dict, photo_type: str, pose_idx: int):
|
||
"""拍摄并上传"""
|
||
point_id = point.get("id", str(point))
|
||
|
||
# 确定 serialNumber
|
||
if photo_type == "front":
|
||
# 正面：从二维码获取 serialNumber
|
||
serial = self.qr_scanner.scan_with_retry(max_attempts=5, interval=0.5)
|
||
if not serial:
|
||
logger.warning(f"点位 {point.get('name')} 正面拍摄未扫描到二维码，跳过")
|
||
return
|
||
self.snapshot_serial_map[point_id] = serial
|
||
else:
|
||
# 背面：使用缓存的 serialNumber
|
||
serial = self.snapshot_serial_map.get(point_id)
|
||
if not serial:
|
||
logger.warning(f"点位 {point.get('name')} 背面拍摄但无缓存 serialNumber")
|
||
return
|
||
|
||
# 拍摄图片（AGV 端摄像头）
|
||
frame = self.qr_scanner.read_frame()
|
||
if frame is None:
|
||
logger.error("摄像头读取失败")
|
||
return
|
||
|
||
# 保存图片
|
||
photo_dir = os.path.join(os.path.dirname(__file__), "..", "photos")
|
||
os.makedirs(photo_dir, exist_ok=True)
|
||
photo_path = os.path.join(photo_dir, f"{serial}_{photo_type}_{int(time.time())}.jpg")
|
||
import cv2
|
||
cv2.imwrite(photo_path, frame)
|
||
|
||
# 上传
|
||
self.uploader.upload(photo_path, serial, pose_idx, photo_type)
|
||
logger.info(f"上传完成: {serial} {photo_type}")
|
||
|
||
# ========== Nav2 导航 ==========
|
||
|
||
def _nav2_check_available(self) -> bool:
|
||
"""检查 Nav2 action server 是否可用"""
|
||
try:
|
||
rc, out, err = self._run_ros2_cmd("ros2 action list")
|
||
if rc != 0:
|
||
return False
|
||
return "/navigate_to_pose" in out or "navigate_to_pose" in out
|
||
except:
|
||
return False
|
||
|
||
def _nav2_send_goal(self, x: float, y: float, yaw: float, timeout_sec: float = 120.0) -> bool:
|
||
"""
|
||
通过 ros2 action 发送 navigate_to_pose 目标并等待结果
|
||
|
||
Returns:
|
||
是否成功到达
|
||
"""
|
||
# 检查 Nav2 可用性
|
||
if not self._nav2_check_available():
|
||
logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动")
|
||
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" 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}"
|
||
)
|
||
|
||
logger.info(f"Nav2 发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
|
||
|
||
# 发送 goal 并监听反馈和结果
|
||
cmd = (
|
||
f"ros2 action send_goal /navigate_to_pose "
|
||
f"navigation_action_msgs/NavigateToPose "
|
||
f"'{pose_yaml}' "
|
||
f"--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
|
||
check_interval = 1.0
|
||
elapsed = 0.0
|
||
|
||
while elapsed < timeout_sec:
|
||
import select
|
||
reads = [process.stdout]
|
||
ready, _, _ = select.select(reads, [], [], check_interval)
|
||
elapsed += check_interval
|
||
|
||
if process.stdout in ready:
|
||
line = process.stdout.readline()
|
||
if not line:
|
||
break
|
||
line_stripped = line.strip()
|
||
|
||
if "succeeded" in line_stripped.lower():
|
||
logger.info("✅ Nav2 导航成功到达目标")
|
||
succeeded = True
|
||
break
|
||
elif "failed" in line_stripped.lower() or "aborted" in line_stripped.lower():
|
||
logger.warning(f"⚠️ Nav2 导航失败: {line_stripped}")
|
||
break
|
||
elif "canceled" in line_stripped.lower() or "cancelled" in line_stripped.lower():
|
||
logger.info("Nav2 导航被取消")
|
||
break
|
||
|
||
if process.poll() is not None:
|
||
# 进程结束但没读到 succeeded
|
||
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_go_to_point(self, x: float, y: float, yaw: float = 0.0,
|
||
timeout_sec: float = 120.0) -> bool:
|
||
"""
|
||
AGV 通过 Nav2 导航到目标点位
|
||
|
||
Args:
|
||
x, y: 目标世界坐标（米）
|
||
yaw: 目标朝向（弧度）
|
||
timeout_sec: 超时时间
|
||
|
||
Returns:
|
||
是否成功到达
|
||
"""
|
||
return self._nav2_send_goal(x, y, yaw, timeout_sec)
|
||
|
||
def _nav2_cancel(self):
|
||
"""取消当前 Nav2 导航"""
|
||
cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true'"
|
||
try:
|
||
subprocess.run(cancel_cmd, shell=True, timeout=3)
|
||
logger.info("Nav2 导航已取消")
|
||
except:
|
||
pass
|
||
|
||
def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
|
||
"""执行 ros2 命令"""
|
||
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)
|
||
|
||
# ========== 状态查询 ==========
|
||
|
||
def get_status(self) -> dict:
|
||
return {
|
||
"task_status": self.status.value,
|
||
"current_point": self.current_point_index,
|
||
"current_pose": self.current_pose_index,
|
||
"agv_connected": self.agv.is_connected(),
|
||
"arm_connected": self.arm_client is not None,
|
||
"camera_opened": self.qr_scanner._cap is not None and self.qr_scanner._cap.isOpened()
|
||
}
|
||
|
||
def pause(self):
|
||
self.status = TaskStatus.PAUSED
|
||
|
||
def resume(self):
|
||
self.status = TaskStatus.RUNNING
|
||
|
||
def stop(self):
|
||
if self.arm_client:
|
||
self.arm_client.task_stop()
|
||
self.agv.stop()
|
||
self.status = TaskStatus.PENDING |