package com.algo.service;
|
|
import com.algo.config.EnvDataConfig;
|
import com.algo.model.*;
|
import com.algo.util.JsonUtils;
|
import com.algo.util.PathTimeCalculator;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.stereotype.Service;
|
|
import javax.annotation.PostConstruct;
|
import java.util.*;
|
import java.util.concurrent.*;
|
|
/**
|
* 路径规划服务
|
*/
|
@Service
|
public class PathPlanningService {
|
|
@Autowired
|
private EnvDataConfig envDataConfig;
|
|
/**
|
* 执行中任务提取
|
*/
|
private ExecutingTaskExtractor taskExtractor;
|
|
/**
|
* 路径规划
|
*/
|
private PathPlanner pathPlanner;
|
|
/**
|
* 碰撞检测
|
*/
|
private CollisionDetector collisionDetector;
|
|
/**
|
* 碰撞解决
|
*/
|
private CollisionResolver collisionResolver;
|
|
/**
|
* 剩余路径处理
|
*/
|
private RemainingPathProcessor remainingPathProcessor;
|
|
/**
|
* 路径时间计算器
|
*/
|
private PathTimeCalculator timeCalculator;
|
|
|
/**
|
* 线程池
|
*/
|
private final ExecutorService executorService = Executors.newFixedThreadPool(Math.max(4, Runtime.getRuntime().availableProcessors()));
|
|
/**
|
* CTU批处理大小
|
*/
|
private final int batchSize = 10;
|
|
|
/**
|
* 初始化各个组件
|
*/
|
@PostConstruct
|
public void initializeComponents() {
|
|
// 初始化路径规划器
|
this.pathPlanner = new AStarPathPlanner(envDataConfig.getPathMapping());
|
|
// 初始化碰撞检测器
|
this.collisionDetector = new CollisionDetector(envDataConfig.getPathMapping());
|
|
// 初始化碰撞解决器
|
this.collisionResolver = new CollisionResolver(collisionDetector);
|
|
// 初始化剩余路径处理器
|
this.remainingPathProcessor = new RemainingPathProcessor(envDataConfig.getPathMapping());
|
|
// 初始化时间计算器
|
Map<String, double[]> realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
|
this.timeCalculator = new PathTimeCalculator(envDataConfig.getPathMapping(), realCoordinateMapping);
|
|
// 为碰撞解决器设置时间计算器、路径规划器和路径映射表
|
this.collisionResolver.setTimeCalculator(timeCalculator);
|
this.collisionResolver.setPathPlanner((PathPlanner) pathPlanner);
|
this.collisionResolver.setPathMapping(envDataConfig.getPathMapping());
|
}
|
|
/**
|
* 为所有CTU规划路径
|
*
|
* @param agvStatusList CTU状态列表
|
* @param includeIdleAgv 是否包含空闲CTU
|
* @param constraints 路径约束条件
|
* @return 路径规划结果
|
*/
|
public PathPlanningResult planAllAgvPaths(List<TaskData> taskList, List<AGVStatus> agvStatusList,
|
boolean includeIdleAgv,
|
List<double[]> constraints) {
|
// 初始化任务提取器
|
this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);
|
|
long startTime = System.currentTimeMillis();
|
long unifiedTimestamp = startTime / 1000;
|
|
System.out.println("开始为 " + agvStatusList.size() + " 个CTU规划");
|
System.out.println("时间基准: " + unifiedTimestamp + "秒 (" + new Date(startTime) + ")");
|
|
// 统计故障车数量
|
long faultyCount = agvStatusList.stream().filter(agv -> agv.getError() == 1).count();
|
if (faultyCount > 0) {
|
System.out.println("检测到 " + faultyCount + " 个故障车(error)，将在时空表中占据位置");
|
}
|
|
// 1. 构建现有剩余路径的时空占用表
|
System.out.println("步骤1: 构建时空占用表");
|
Map<String, String> spaceTimeOccupancyMap = remainingPathProcessor.buildSpaceTimeOccupancyMap(agvStatusList, unifiedTimestamp);
|
System.out.println("时空占用表构建完成，包含 " + spaceTimeOccupancyMap.size() + " 个占用点");
|
|
// 2. 初始化规划结果集合
|
List<PlannedPath> plannedPaths = new ArrayList<>();
|
List<ExecutingTask> executingTasks = new ArrayList<>();
|
Map<String, String> plannedAgvIds = new HashMap<>(); // 提前初始化标记故障车
|
|
// 3. 分类处理CTU：有剩余路径的和需要新路径的
|
List<AGVStatus> ctuWithRemainingPaths = new ArrayList<>();
|
List<AGVStatus> ctuNeedingNewPaths = new ArrayList<>();
|
|
for (AGVStatus agv : agvStatusList) {
|
// 故障车已在时空占用表中处理，标记为已处理但不规划路径
|
if (agv.getError() == 1) {
|
plannedAgvIds.put(agv.getAgvId(), "FAULTY");
|
continue;
|
}
|
|
if (agv.hasRemainingPath()) {
|
// 有剩余路径的AGV，先处理剩余路径
|
ctuWithRemainingPaths.add(agv);
|
} else {
|
// 只要不是故障车且没有剩余路径，参与路径规划
|
ctuNeedingNewPaths.add(agv);
|
}
|
}
|
|
System.out.println("CTU分类完成: 有剩余路径=" + ctuWithRemainingPaths.size() +
|
", 需要新路径=" + ctuNeedingNewPaths.size());
|
|
// 记录需要新路径的AGV信息（debug）
|
if (!ctuNeedingNewPaths.isEmpty()) {
|
System.out.println("需要新路径的AGV详情:");
|
for (AGVStatus agv : ctuNeedingNewPaths) {
|
System.out.println(" - AGV " + agv.getAgvId() +
|
": error=" + agv.getError() +
|
", status=" + agv.getStatus() +
|
", hasRemainingPath=" + agv.hasRemainingPath() +
|
", isAvailable=" + agv.isAvailable() +
|
", position=" + agv.getPosition());
|
}
|
}
|
|
// 4. 处理有剩余路径的CTU
|
|
System.out.println("步骤2: 处理有剩余路径的CTU");
|
for (AGVStatus agv : ctuWithRemainingPaths) {
|
PlannedPath remainingPath = processRemainingPath(agv, unifiedTimestamp);
|
if (remainingPath != null) {
|
plannedPaths.add(remainingPath);
|
plannedAgvIds.put(agv.getAgvId(), "REMAINING_PATH");
|
|
// 创建对应的执行任务
|
ExecutingTask task = createExecutingTaskFromRemainingPath(agv);
|
if (task != null) {
|
executingTasks.add(task);
|
}
|
|
System.out.println("CTU " + agv.getAgvId() + " 剩余路径处理完成，路径长度: " +
|
remainingPath.getCodeList().size());
|
}
|
}
|
|
// 4.5. 检查空闲AGV是否需要让行
|
System.out.println("检查空闲AGV是否需要让行");
|
List<AGVStatus> yieldingAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
|
if (!yieldingAgvs.isEmpty()) {
|
System.out.println(" 发现 " + yieldingAgvs.size() + " 个需要让行的空闲AGV");
|
for (AGVStatus yieldAgv : yieldingAgvs) {
|
PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
|
if (yieldPath != null) {
|
plannedPaths.add(yieldPath);
|
plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
|
System.out.println(" AGV " + yieldAgv.getAgvId() + " 让行路径规划成功，从 " +
|
yieldAgv.getPosition() + " 移开");
|
}
|
}
|
} else {
|
System.out.println(" 无需让行的AGV");
|
}
|
|
// 5. 为需要新路径的CTU提取执行中任务
|
System.out.println("步骤3: 提取需要新路径的CTU任务");
|
List<ExecutingTask> newTasks = taskExtractor.extractExecutingTasks(ctuNeedingNewPaths);
|
System.out.println("提取到 " + newTasks.size() + " 个新任务");
|
|
// 6. 为新任务规划路径（考虑时空约束）
|
System.out.println("步骤4: 为新任务规划路径");
|
for (ExecutingTask task : newTasks) {
|
String agvId = task.getAgvId();
|
String currentPos = task.getCurrentPosition();
|
String targetPos = task.getTargetPosition();
|
|
// 避免重复规划
|
if (plannedAgvIds.containsKey(agvId)) {
|
System.out.println("跳过CTU " + agvId + "：已规划路径");
|
continue;
|
}
|
|
// 验证位置信息
|
if (currentPos == null || targetPos == null) {
|
System.out.println("CTU " + agvId + " 位置信息无效，跳过");
|
continue;
|
}
|
|
// 获取CTU状态信息
|
AGVStatus agvStatus = findAgvStatus(agvId, ctuNeedingNewPaths);
|
if (agvStatus == null) {
|
System.out.println("CTU " + agvId + " 状态信息未找到，跳过");
|
continue;
|
}
|
|
// 规划时空安全的路径
|
PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
|
currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
|
);
|
|
if (plannedPath != null) {
|
// 设置CTU信息
|
plannedPath.setAgvId(agvId);
|
plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));
|
|
enhancePathCodes(plannedPath, task);
|
|
plannedPaths.add(plannedPath);
|
plannedAgvIds.put(agvId, targetPos);
|
executingTasks.add(task);
|
|
// 更新时空占用表
|
updateSpaceTimeOccupancyMap(plannedPath, spaceTimeOccupancyMap, agvStatus);
|
|
System.out.println("CTU " + agvId + " 路径规划成功：" + currentPos + " -> " + targetPos +
|
"，路径长度: " + plannedPath.getCodeList().size());
|
} else {
|
System.out.println("CTU " + agvId + " 路径规划失败：" + currentPos + " -> " + targetPos);
|
}
|
}
|
|
// 7. 处理空闲CTU
|
if (includeIdleAgv) {
|
System.out.println("步骤5: 处理空闲CTU");
|
|
// 7.1. 检查空闲AGV是否占用已规划路径，如果占用则生成避让路径
|
List<AGVStatus> yieldingIdleAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
|
|
List<AGVStatus> yieldingAgvsToHandle = new ArrayList<>();
|
for (AGVStatus yieldAgv : yieldingIdleAgvs) {
|
if (!plannedAgvIds.containsKey(yieldAgv.getAgvId())) {
|
yieldingAgvsToHandle.add(yieldAgv);
|
}
|
}
|
|
if (!yieldingAgvsToHandle.isEmpty()) {
|
System.out.println( yieldingAgvsToHandle.size() + " 个需要避让的空闲AGV");
|
for (AGVStatus yieldAgv : yieldingAgvsToHandle) {
|
PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
|
if (yieldPath != null) {
|
plannedPaths.add(yieldPath);
|
plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
|
}
|
}
|
} else {
|
System.out.println(" 无需避让的空闲AGV");
|
}
|
|
// 7.2. 然后为剩余的空闲AGV规划待机/充电路径
|
List<PlannedPath> idlePaths = planIdleAgvPathsWithConstraints(
|
ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap, unifiedTimestamp
|
);
|
plannedPaths.addAll(idlePaths);
|
System.out.println(" 空闲CTU路径规划，数量: " + idlePaths.size());
|
}
|
|
// 7.5. 最终避让检查
|
performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
|
|
// 7.6. 重新计算所有路径的时间戳
|
recalculateAllPathTimings(plannedPaths, agvStatusList, unifiedTimestamp);
|
|
// 8. 最终碰撞检测和解决
|
System.out.println("步骤6: 最终碰撞检测");
|
List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);
|
|
if (!conflicts.isEmpty()) {
|
System.out.println("发现 " + conflicts.size() + " 个碰撞，开始解决");
|
plannedPaths = collisionResolver.resolveConflicts(plannedPaths, conflicts, executingTasks, agvStatusList, unifiedTimestamp);
|
|
// 重新检测冲突
|
List<Conflict> remainingConflicts = collisionDetector.detectConflicts(plannedPaths);
|
conflicts = remainingConflicts;
|
}
|
|
long endTime = System.currentTimeMillis();
|
double totalTime = (endTime - startTime) / 1000.0;
|
|
// 构建结果
|
PathPlanningResult result = new PathPlanningResult(
|
agvStatusList.size(),
|
executingTasks.size(),
|
plannedPaths.size(),
|
totalTime,
|
conflicts.size(),
|
conflicts.isEmpty(),
|
"A*_WITH_SPACETIME_CONSTRAINTS",
|
plannedPaths,
|
executingTasks
|
);
|
|
System.out.println("路径规划完成 - 总CTU: " + result.getTotalAgvs() +
|
", 执行任务: " + result.getExecutingTasksCount() +
|
", 规划路径: " + result.getPlannedPathsCount() +
|
", 耗时: " + result.getTotalPlanningTime() + "s" +
|
", 无冲突: " + result.isCollisionFree());
|
|
return result;
|
}
|
|
/**
|
* 处理CTU的剩余路径
|
*
|
* @param agv CTU状态
|
* @param unifiedTimestamp 统一时间戳（秒）
|
* @return 处理后的路径
|
*/
|
private PlannedPath processRemainingPath(AGVStatus agv, long unifiedTimestamp) {
|
if (!agv.hasRemainingPath()) {
|
return null;
|
}
|
|
PlannedPath remainingPath = agv.getRemainingPath();
|
List<PathCode> remainingCodes = new ArrayList<>();
|
|
// 获取剩余路径
|
List<PathCode> originalCodes = remainingPath.getCodeList();
|
for (int i = agv.getCurrentPathIndex(); i < originalCodes.size(); i++) {
|
PathCode originalCode = originalCodes.get(i);
|
PathCode newCode = new PathCode(originalCode.getCode(), originalCode.getDirection());
|
newCode.setActionType(originalCode.getActionType());
|
newCode.setTaskId(originalCode.getTaskId());
|
newCode.setPosType(originalCode.getPosType());
|
newCode.setLev(originalCode.getLev());
|
newCode.setTargetPoint(originalCode.isTargetPoint());
|
remainingCodes.add(newCode);
|
}
|
|
PlannedPath processedPath = new PlannedPath();
|
processedPath.setAgvId(agv.getAgvId());
|
processedPath.setCodeList(remainingCodes);
|
|
// 使用输入中的segId，如果没有则生成新的
|
String segId = remainingPath.getSegId();
|
if (segId == null || segId.trim().isEmpty()) {
|
segId = agv.getAgvId() + "_REMAINING_" + System.currentTimeMillis();
|
}
|
processedPath.setSegId(segId);
|
|
if (timeCalculator != null && !remainingCodes.isEmpty()) {
|
// 使用统一时间戳作为起始时间（转换为毫秒）
|
long startTime = unifiedTimestamp * 1000;
|
|
CTUPhysicalConfig config = agv.getPhysicalConfig();
|
|
// 估算当前速度（AGV移动中为正常速度；静止为0）
|
double initialSpeed = agv.hasRemainingPath() && agv.getRemainingPathLength() > 0
|
? config.getNormalSpeed() : 0.0;
|
|
// 计算时间信息
|
// arrivalTime, departureTime, cumulativeTime
|
timeCalculator.calculatePathTiming(
|
processedPath,
|
startTime,
|
config,
|
initialSpeed
|
);
|
} else {
|
System.out.println(" 未能为剩余路径设置时间信息 - AGV: " + agv.getAgvId());
|
}
|
|
return processedPath;
|
}
|
|
/**
|
* 从剩余路径创建执行任务
|
*
|
* @param agv CTU状态
|
* @return 执行任务
|
*/
|
private ExecutingTask createExecutingTaskFromRemainingPath(AGVStatus agv) {
|
if (!agv.hasRemainingPath()) {
|
return null;
|
}
|
|
PlannedPath remainingPath = agv.getRemainingPath();
|
List<PathCode> codeList = remainingPath.getCodeList();
|
|
if (codeList.isEmpty() || agv.getCurrentPathIndex() >= codeList.size()) {
|
return null;
|
}
|
|
// 获取当前位置和目标位置
|
PathCode currentCode = codeList.get(agv.getCurrentPathIndex());
|
PathCode targetCode = codeList.get(codeList.size() - 1);
|
|
ExecutingTask task = new ExecutingTask();
|
task.setAgvId(agv.getAgvId());
|
task.setTaskId(remainingPath.getSegId());
|
task.setCurrentPosition(currentCode.getCode());
|
task.setTargetPosition(targetCode.getCode());
|
task.setTaskType("remaining");
|
task.setLoaded(false); // 假设值
|
task.setBackpackIndex(0);
|
task.setPriority(1);
|
|
return task;
|
}
|
|
/**
|
* 规划带时空约束的路径
|
*
|
* @param startPos 起始位置
|
* @param endPos 目标位置
|
* @param constraints 约束条件
|
* @param occupancyMap 时空占用表
|
* @param agvStatus CTU状态
|
* @param unifiedTimestamp 统一时间戳（秒）
|
* @return 规划的路径
|
*/
|
private PlannedPath planPathWithSpaceTimeConstraints(String startPos, String endPos,
|
List<double[]> constraints,
|
Map<String, String> occupancyMap,
|
AGVStatus agvStatus,
|
long unifiedTimestamp) {
|
// 带时空占用表的路径规划
|
long startTimeMs = unifiedTimestamp * 1000;
|
PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
|
startPos,
|
endPos,
|
constraints,
|
occupancyMap,
|
agvStatus.getPhysicalConfig(),
|
startTimeMs
|
);
|
|
if (spacetimePath == null) {
|
return null;
|
}
|
|
timeCalculator.calculatePathTiming(spacetimePath, startTimeMs, agvStatus.getPhysicalConfig(), 0.0);
|
|
return spacetimePath;
|
}
|
|
/**
|
* 路径的时间信息
|
*
|
* @param path 路径
|
* @param startTime 起始时间
|
* @param config 物理配置
|
*/
|
private void enhancePathWithTimeInfo(PlannedPath path, long startTime, CTUPhysicalConfig config) {
|
List<PathCode> codeList = path.getCodeList();
|
if (codeList == null || codeList.isEmpty()) {
|
return;
|
}
|
|
long currentTime = startTime;
|
|
for (int i = 0; i < codeList.size(); i++) {
|
PathCode pathCode = codeList.get(i);
|
|
// 设置预计到达时间
|
|
// 计算到下一个点的时间
|
if (i < codeList.size() - 1) {
|
double travelTime = config.getStandardPointDistance() / config.getNormalSpeed();
|
currentTime += (long) (travelTime * 1000);
|
|
// 如果有方向变化，增加转向时间
|
PathCode nextCode = codeList.get(i + 1);
|
// 添加null检查
|
if (pathCode.getDirection() != null && nextCode.getDirection() != null) {
|
if (!pathCode.getDirection().equals(nextCode.getDirection())) {
|
double turnTime = config.getTurnTime(pathCode.getDirection(), nextCode.getDirection());
|
currentTime += (long) (turnTime * 1000);
|
}
|
}
|
}
|
}
|
}
|
|
/**
|
* 更新时空占用表
|
*
|
* @param path 新路径
|
* @param occupancyMap 占用表
|
* @param agvStatus CTU状态
|
*/
|
private void updateSpaceTimeOccupancyMap(PlannedPath path, Map<String, String> occupancyMap,
|
AGVStatus agvStatus) {
|
// 将新路径添加到占用表中
|
List<PathCode> codeList = path.getCodeList();
|
CTUPhysicalConfig config = agvStatus.getPhysicalConfig();
|
|
long currentTime = System.currentTimeMillis() / 1000; // 转换为秒
|
|
for (PathCode pathCode : codeList) {
|
int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), envDataConfig.getPathMapping());
|
if (coord != null) {
|
String spaceTimeKey = coord[0] + "," + coord[1] + "," + currentTime;
|
occupancyMap.put(spaceTimeKey, agvStatus.getAgvId());
|
currentTime += 2; // 假设每个点停留2秒
|
}
|
}
|
}
|
|
/**
|
* 查找CTU状态
|
*
|
* @param agvId CTU编号
|
* @param agvList CTU列表
|
* @return CTU状态
|
*/
|
private AGVStatus findAgvStatus(String agvId, List<AGVStatus> agvList) {
|
for (AGVStatus agv : agvList) {
|
if (agv.getAgvId().equals(agvId)) {
|
return agv;
|
}
|
}
|
return null;
|
}
|
|
/**
|
* 规划带约束的空闲CTU路径
|
*
|
* @param agvStatusList CTU状态列表
|
* @param plannedAgvIds 已规划CTU映射
|
* @param constraints 约束条件
|
* @param occupancyMap 时空占用表
|
* @param unifiedTimestamp 统一时间戳（秒）
|
* @return 空闲CTU路径列表
|
*/
|
private List<PlannedPath> planIdleAgvPathsWithConstraints(List<AGVStatus> agvStatusList,
|
Map<String, String> plannedAgvIds,
|
List<double[]> constraints,
|
Map<String, String> occupancyMap,
|
long unifiedTimestamp) {
|
List<PlannedPath> idlePaths = new ArrayList<>();
|
|
for (AGVStatus agvStatus : agvStatusList) {
|
String agvId = agvStatus.getAgvId();
|
|
// 跳过已规划的CTU
|
if (plannedAgvIds.containsKey(agvId)) {
|
continue;
|
}
|
|
// 检查CTU是否空闲
|
if (isAgvIdle(agvStatus)) {
|
String currentPos = agvStatus.getPosition();
|
String targetPos = getIdleAgvTarget(agvId, currentPos);
|
|
if (targetPos != null && !targetPos.equals(currentPos)) {
|
PlannedPath idlePath = planPathWithSpaceTimeConstraints(
|
currentPos, targetPos, constraints, occupancyMap, agvStatus, unifiedTimestamp
|
);
|
|
if (idlePath != null) {
|
idlePath.setAgvId(agvId);
|
idlePath.setSegId(generateSegId("IDLE", agvId, "idle"));
|
|
// 设置空闲路径的代码信息
|
enhanceIdlePathCodes(idlePath, agvStatus);
|
|
idlePaths.add(idlePath);
|
plannedAgvIds.put(agvId, targetPos);
|
|
// 更新占用表
|
updateSpaceTimeOccupancyMap(idlePath, occupancyMap, agvStatus);
|
|
System.out.println("空闲CTU " + agvId + " 路径规划成功：" + currentPos + " -> " + targetPos);
|
}
|
}
|
}
|
}
|
|
return idlePaths;
|
}
|
|
/**
|
* 路径代码信息
|
*
|
* @param plannedPath 规划路径
|
* @param task 执行任务
|
*/
|
private void enhancePathCodes(PlannedPath plannedPath, ExecutingTask task) {
|
List<PathCode> codeList = plannedPath.getCodeList();
|
if (codeList == null || codeList.isEmpty()) {
|
return;
|
}
|
|
String taskId = task.getTaskId();
|
int backpackLevel = task.getBackpackIndex();
|
|
for (int i = 0; i < codeList.size(); i++) {
|
PathCode pathCode = codeList.get(i);
|
|
// 设置基本信息
|
pathCode.setTaskId(taskId);
|
pathCode.setLev(backpackLevel);
|
|
// 设置目标点信息
|
if (i == codeList.size() - 1) { // 最后一个点
|
pathCode.setTargetPoint(true);
|
// 根据任务类型设置actionType和posType（
|
if (task.isLoaded()) {
|
pathCode.setActionType("2"); // 放货
|
pathCode.setPosType("2"); // 放货
|
} else {
|
pathCode.setActionType("1"); // 取货
|
pathCode.setPosType("1"); // 取货
|
}
|
} else {
|
pathCode.setTargetPoint(false);
|
pathCode.setActionType(null);
|
pathCode.setPosType(null);
|
}
|
}
|
}
|
|
/**
|
* 空闲路径代码信息
|
*
|
* @param plannedPath 规划路径
|
* @param agvStatus CTU状态
|
*/
|
private void enhanceIdlePathCodes(PlannedPath plannedPath, AGVStatus agvStatus) {
|
List<PathCode> codeList = plannedPath.getCodeList();
|
if (codeList == null || codeList.isEmpty()) {
|
return;
|
}
|
|
String actionType = agvStatus.needsCharging() ? "3" : "4"; // 充电或待机
|
|
for (PathCode pathCode : codeList) {
|
pathCode.setActionType(actionType);
|
pathCode.setTaskId(null);
|
pathCode.setPosType(null);
|
pathCode.setLev(0);
|
pathCode.setTargetPoint(false);
|
}
|
}
|
|
/**
|
* 判断CTU是否空闲
|
*
|
* @param agvStatus CTU状态
|
* @return true如果CTU空闲
|
*/
|
private boolean isAgvIdle(AGVStatus agvStatus) {
|
if (!agvStatus.isAvailable()) {
|
return false;
|
}
|
|
// 检查背篓是否有执行中的任务
|
List<BackpackData> backpack = agvStatus.getBackpack();
|
if (backpack != null) {
|
for (BackpackData bp : backpack) {
|
if (bp.getTaskId() != null && !bp.getTaskId().trim().isEmpty()) {
|
return false;
|
}
|
}
|
}
|
|
return true;
|
}
|
|
/**
|
* 获取空闲CTU的目标位置
|
*
|
* @param agvId CTU编号
|
* @param currentPosition 当前位置
|
* @return 目标位置
|
*/
|
private String getIdleAgvTarget(String agvId, String currentPosition) {
|
// 根据CTU ID的哈希值选择位置
|
int hashValue = Math.abs(agvId.hashCode() % 1000);
|
|
// 优先选择充电位置
|
List<String> chargingPositions = taskExtractor.getChargingPositions();
|
if (!chargingPositions.isEmpty()) {
|
for (String pos : chargingPositions) {
|
if (!pos.equals(currentPosition)) {
|
return pos;
|
}
|
}
|
}
|
|
// 其次选择待机位置
|
List<String> standbyPositions = taskExtractor.getStandbyPositions();
|
if (!standbyPositions.isEmpty()) {
|
return standbyPositions.get(hashValue % standbyPositions.size());
|
}
|
|
return null;
|
}
|
|
/**
|
* 生成段落ID
|
*
|
* @param taskId 任务ID
|
* @param agvId CTU编号
|
* @param taskType 任务类型
|
* @return 段落ID
|
*/
|
private String generateSegId(String taskId, String agvId, String taskType) {
|
return taskId + "_" + agvId + "_" + taskType;
|
}
|
|
/**
|
* 迭代检查所有空闲AGV是否占用已规划路径
|
*/
|
private void performFinalYieldingCheck(List<AGVStatus> agvStatusList,
|
List<PlannedPath> plannedPaths,
|
Map<String, String> plannedAgvIds,
|
Map<String, String> spaceTimeOccupancyMap,
|
List<double[]> constraints,
|
long unifiedTimestamp) {
|
final int MAX_ITERATIONS = 5; // 最大迭代次数
|
int iteration = 0;
|
int totalYieldingCount = 0;
|
|
while (iteration < MAX_ITERATIONS) {
|
iteration++;
|
|
// 识别需要避让的空闲AGV
|
List<AGVStatus> yieldingAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
|
|
if (yieldingAgvs.isEmpty()) {
|
if (iteration == 1) {
|
System.out.println(" 无需避让的AGV");
|
} else {
|
System.out.println(" 第" + iteration + "轮检查：无新的避让需求");
|
}
|
break;
|
}
|
|
System.out.println(" 第" + iteration + "轮检查：" + yieldingAgvs.size() + " 个需要避让的AGV");
|
|
// 规划避让路径
|
int successCount = 0;
|
for (AGVStatus yieldAgv : yieldingAgvs) {
|
if (plannedAgvIds.containsKey(yieldAgv.getAgvId())) {
|
continue;
|
}
|
|
PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
|
if (yieldPath != null) {
|
plannedPaths.add(yieldPath);
|
plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
|
successCount++;
|
totalYieldingCount++;
|
}
|
}
|
|
if (successCount == 0) {
|
System.out.println(" 第" + iteration + "轮检查：无法为需要避让的AGV规划路径，停止迭代");
|
break;
|
}
|
}
|
|
if (iteration >= MAX_ITERATIONS) {
|
System.out.println(" 达到最大迭代次数(" + MAX_ITERATIONS + ")，可能存在复杂的避让场景");
|
}
|
|
if (totalYieldingCount > 0) {
|
System.out.println(" 避让检查完成：共处理 " + totalYieldingCount + " 个避让AGV，迭代 " + iteration + " 轮");
|
}
|
}
|
|
private void recalculateAllPathTimings(List<PlannedPath> plannedPaths,
|
List<AGVStatus> agvStatusList,
|
long unifiedTimestamp) {
|
if (plannedPaths == null || plannedPaths.isEmpty() || timeCalculator == null) {
|
return;
|
}
|
|
Map<String, AGVStatus> agvStatusMap = new HashMap<>();
|
for (AGVStatus agv : agvStatusList) {
|
if (agv.getAgvId() != null) {
|
agvStatusMap.put(agv.getAgvId(), agv);
|
}
|
}
|
|
long startTimeMs = unifiedTimestamp * 1000;
|
int recalculatedCount = 0;
|
|
// 遍历所有路径，重新计算时间戳
|
for (PlannedPath path : plannedPaths) {
|
if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
|
continue;
|
}
|
|
String agvId = path.getAgvId();
|
if (agvId == null) {
|
continue;
|
}
|
|
// 获取AGV的物理配置
|
AGVStatus agvStatus = agvStatusMap.get(agvId);
|
CTUPhysicalConfig config = (agvStatus != null && agvStatus.getPhysicalConfig() != null)
|
? agvStatus.getPhysicalConfig()
|
: new CTUPhysicalConfig();
|
|
double initialSpeed = 0.0;
|
|
// 重新计算路径时间戳
|
timeCalculator.calculatePathTiming(path, startTimeMs, config, initialSpeed);
|
recalculatedCount++;
|
}
|
|
System.out.println(" 已重新计算 " + recalculatedCount + " 条路径的时间戳（统一时间基准: " + unifiedTimestamp + "秒）");
|
}
|
|
/**
|
* 识别需要让行的AGV
|
* 检查空闲AGV是否占用了其他AGV剩余路径上的位置
|
*
|
* @param agvStatusList 所有AGV状态列表
|
* @param plannedPaths 已规划的路径列表（包含剩余路径）
|
* @return 需要让行的AGV列表
|
*/
|
private List<AGVStatus> identifyYieldingAgvs(List<AGVStatus> agvStatusList, List<PlannedPath> plannedPaths) {
|
List<AGVStatus> yieldingAgvs = new ArrayList<>();
|
|
// 收集所有已规划路径上的位置
|
Set<String> occupiedPositions = new HashSet<>();
|
for (PlannedPath path : plannedPaths) {
|
if (path.getCodeList() != null) {
|
for (PathCode code : path.getCodeList()) {
|
if (code.getCode() != null) {
|
occupiedPositions.add(code.getCode());
|
}
|
}
|
}
|
}
|
|
// 检查每个AGV
|
for (AGVStatus agv : agvStatusList) {
|
// 如果这个AGV已经有剩余路径，跳过（已在任务）
|
if (agv.hasRemainingPath()) {
|
continue;
|
}
|
|
// 检查AGV是否有位置信息
|
if (agv.getPosition() == null || agv.getPosition().trim().isEmpty()) {
|
continue;
|
}
|
|
// 检查AGV是否是空闲状态（status=0）
|
// 只要没有剩余路径且有位置，就检查是否需要让行
|
int status = agv.getStatus();
|
if (status != 0) {
|
// status=1或2（问题或忙碌）等其他状态跳过
|
continue;
|
}
|
|
// 检查该AGV的位置是否在其他AGV的路径上
|
String currentPos = agv.getPosition();
|
if (occupiedPositions.contains(currentPos)) {
|
System.out.println(" CTU " + agv.getAgvId() +
|
" (status=" + status + ") 在位置 " + currentPos + " 占用了其他CTU路径，需要让行");
|
yieldingAgvs.add(agv);
|
}
|
}
|
|
return yieldingAgvs;
|
}
|
|
/**
|
* 为让行AGV规划避让路径
|
*
|
* @param yieldAgv 需要让行的AGV
|
* @param existingPaths 已存在的路径列表
|
* @param occupancyMap 时空占用表
|
* @param constraints 路径约束
|
* @return 让行路径
|
*/
|
private PlannedPath planYieldPath(AGVStatus yieldAgv, List<PlannedPath> existingPaths,
|
Map<String, String> occupancyMap, List<double[]> constraints,
|
long unifiedTimestamp) {
|
String currentPos = yieldAgv.getPosition();
|
String agvId = yieldAgv.getAgvId();
|
|
// 1. 找到安全的目标位置
|
Set<String> blockedPositions = new HashSet<>();
|
for (PlannedPath path : existingPaths) {
|
if (path.getCodeList() != null) {
|
for (PathCode code : path.getCodeList()) {
|
if (code.getCode() != null) {
|
blockedPositions.add(code.getCode());
|
}
|
}
|
}
|
}
|
|
// 2. 寻找合适让行目标位置
|
String targetPos = findYieldTargetPosition(currentPos, blockedPositions, yieldAgv);
|
|
if (targetPos == null || targetPos.equals(currentPos)) {
|
System.out.println(" AGV " + agvId + " 无法找到合适的让行位置");
|
return null;
|
}
|
|
// 3. 规划避让路径
|
PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
|
currentPos, targetPos, constraints, occupancyMap, yieldAgv, unifiedTimestamp
|
);
|
|
if (yieldPath != null) {
|
yieldPath.setAgvId(agvId);
|
yieldPath.setSegId(generateSegId("AVOID", agvId, "avoiding"));
|
|
// 设置路径代码信息
|
enhancePathCodesForYielding(yieldPath);
|
|
// 更新占用表
|
updateSpaceTimeOccupancyMap(yieldPath, occupancyMap, yieldAgv);
|
}
|
|
return yieldPath;
|
}
|
|
/**
|
* 寻找让行的目标位置
|
*/
|
private String findYieldTargetPosition(String currentPos, Set<String> blockedPositions, AGVStatus agv) {
|
// 使用BFS搜索满足安全距离的空闲位置
|
final int MAX_SEARCH_DEPTH = 15;
|
|
// 获取安全距离参数（从物理配置中获取，默认3.0）
|
CTUPhysicalConfig config = agv.getPhysicalConfig();
|
double minSafetyDistance = config != null ? config.getMinSafetyDistance() : 3.0;
|
|
// 将安全距离转换为网格步数（假设每个节点间距为1）
|
int safetySteps = (int) Math.ceil(minSafetyDistance);
|
|
Queue<String> queue = new LinkedList<>();
|
Map<String, Integer> visited = new HashMap<>(); // 记录位置和距离
|
|
queue.offer(currentPos);
|
visited.put(currentPos, 0);
|
|
while (!queue.isEmpty()) {
|
String pos = queue.poll();
|
int depth = visited.get(pos);
|
|
if (depth >= MAX_SEARCH_DEPTH) {
|
break;
|
}
|
|
List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
|
for (Map<String, String> neighbor : neighbors) {
|
String neighborPos = neighbor.get("code");
|
|
if (neighborPos == null || visited.containsKey(neighborPos)) {
|
continue;
|
}
|
|
int distanceToPos = depth + 1;
|
visited.put(neighborPos, distanceToPos);
|
|
// 检查该位置是否满足要求
|
if (!blockedPositions.contains(neighborPos) &&
|
isSafeDistanceFromBlockedPositions(neighborPos, blockedPositions, safetySteps)) {
|
System.out.println(" 找到安全避让位置: " + neighborPos);
|
return neighborPos;
|
}
|
|
// 加入队列继续搜索
|
queue.offer(neighborPos);
|
}
|
}
|
|
System.out.println(" 未找到满足安全距离的避让位置，降低安全要求重试");
|
|
// 如果找不到满足安全距离的位置，降低要求再次搜索（至少保证2步距离）
|
return findYieldTargetPositionWithReducedSafety(currentPos, blockedPositions, 2);
|
}
|
|
/**
|
* 检查候选位置与所有被占用位置是否满足安全距离
|
*/
|
private boolean isSafeDistanceFromBlockedPositions(String candidatePos,
|
Set<String> blockedPositions,
|
int minSteps) {
|
// 获取候选位置的坐标
|
int[] candidateCoord = JsonUtils.getCoordinate(candidatePos, envDataConfig.getPathMapping());
|
if (candidateCoord == null) {
|
return false;
|
}
|
|
// 检查与每个被占用位置的距离
|
for (String blockedPos : blockedPositions) {
|
int[] blockedCoord = JsonUtils.getCoordinate(blockedPos, envDataConfig.getPathMapping());
|
if (blockedCoord == null) {
|
continue;
|
}
|
|
// 计算曼哈顿距离
|
int distance = Math.abs(candidateCoord[0] - blockedCoord[0]) +
|
Math.abs(candidateCoord[1] - blockedCoord[1]);
|
|
if (distance < minSteps) {
|
return false;
|
}
|
}
|
|
return true;
|
}
|
|
/**
|
* 使用降低的安全要求查找让行位置（备选）
|
*
|
* @param currentPos 当前位置
|
* @param blockedPositions 被占用的位置集合
|
* @param minSteps 最小步数要求
|
* @return 让行目标位置
|
*/
|
private String findYieldTargetPositionWithReducedSafety(String currentPos,
|
Set<String> blockedPositions,
|
int minSteps) {
|
final int MAX_SEARCH_DEPTH = 10;
|
|
Queue<String> queue = new LinkedList<>();
|
Map<String, Integer> visited = new HashMap<>();
|
|
queue.offer(currentPos);
|
visited.put(currentPos, 0);
|
|
while (!queue.isEmpty()) {
|
String pos = queue.poll();
|
int depth = visited.get(pos);
|
|
if (depth >= MAX_SEARCH_DEPTH) {
|
break;
|
}
|
|
List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
|
for (Map<String, String> neighbor : neighbors) {
|
String neighborPos = neighbor.get("code");
|
|
if (neighborPos == null || visited.containsKey(neighborPos)) {
|
continue;
|
}
|
|
visited.put(neighborPos, depth + 1);
|
|
// 使用降低的安全要求
|
if (!blockedPositions.contains(neighborPos) &&
|
isSafeDistanceFromBlockedPositions(neighborPos, blockedPositions, minSteps)) {
|
System.out.println(" 找到降级安全避让位置: " + neighborPos +
|
" (距离=" + (depth + 1) + "步, 最小安全距离=" + minSteps + "步)");
|
return neighborPos;
|
}
|
|
queue.offer(neighborPos);
|
}
|
}
|
|
System.out.println(" 无法找到避让位置");
|
return null;
|
}
|
|
private void enhancePathCodesForYielding(PlannedPath yieldPath) {
|
if (yieldPath == null || yieldPath.getCodeList() == null) {
|
return;
|
}
|
|
List<PathCode> codes = yieldPath.getCodeList();
|
for (int i = 0; i < codes.size(); i++) {
|
PathCode code = codes.get(i);
|
code.setActionType("0");
|
code.setPosType(null);
|
code.setLev(0);
|
code.setTargetPoint(i == codes.size() - 1);
|
}
|
}
|
|
/**
|
* 路径规划结果类
|
*/
|
public static class PathPlanningResult {
|
private int totalAgvs;
|
private int executingTasksCount;
|
private int plannedPathsCount;
|
private double totalPlanningTime;
|
private int conflictsDetected;
|
private boolean collisionFree;
|
private String algorithm;
|
private List<PlannedPath> plannedPaths;
|
private List<ExecutingTask> executingTasks;
|
|
public PathPlanningResult(int totalAgvs, int executingTasksCount, int plannedPathsCount,
|
double totalPlanningTime, int conflictsDetected, boolean collisionFree,
|
String algorithm, List<PlannedPath> plannedPaths, List<ExecutingTask> executingTasks) {
|
this.totalAgvs = totalAgvs;
|
this.executingTasksCount = executingTasksCount;
|
this.plannedPathsCount = plannedPathsCount;
|
this.totalPlanningTime = totalPlanningTime;
|
this.conflictsDetected = conflictsDetected;
|
this.collisionFree = collisionFree;
|
this.algorithm = algorithm;
|
this.plannedPaths = plannedPaths;
|
this.executingTasks = executingTasks;
|
}
|
|
// Getter方法
|
public int getTotalAgvs() {
|
return totalAgvs;
|
}
|
|
public int getExecutingTasksCount() {
|
return executingTasksCount;
|
}
|
|
public int getPlannedPathsCount() {
|
return plannedPathsCount;
|
}
|
|
public double getTotalPlanningTime() {
|
return totalPlanningTime;
|
}
|
|
public int getConflictsDetected() {
|
return conflictsDetected;
|
}
|
|
public boolean isCollisionFree() {
|
return collisionFree;
|
}
|
|
public String getAlgorithm() {
|
return algorithm;
|
}
|
|
public List<PlannedPath> getPlannedPaths() {
|
return plannedPaths;
|
}
|
|
public List<ExecutingTask> getExecutingTasks() {
|
return executingTasks;
|
}
|
|
@Override
|
public String toString() {
|
return "PathPlanningResult{" +
|
"totalAgvs=" + totalAgvs +
|
", executingTasksCount=" + executingTasksCount +
|
", plannedPathsCount=" + plannedPathsCount +
|
", executingTasksCount=" + executingTasksCount +
|
", totalPlanningTime=" + totalPlanningTime +
|
", conflictsDetected=" + conflictsDetected +
|
", collisionFree=" + executingTasksCount +
|
", algorithm=" + algorithm +
|
", plannedPaths=" + plannedPaths +
|
", executingTasks='" + executingTasks + '\'' +
|
'}';
|
}
|
}
|
|
/**
|
* 并行规划任务路径
|
*/
|
private List<PlannedPath> planTasksInParallel(List<ExecutingTask> tasks,
|
List<double[]> constraints,
|
Map<String, String> spaceTimeOccupancyMap,
|
List<AGVStatus> agvStatusList,
|
long unifiedTimestamp) {
|
|
List<PlannedPath> allPaths = Collections.synchronizedList(new ArrayList<>());
|
|
// 按优先级排序任务
|
List<ExecutingTask> sortedTasks = new ArrayList<>(tasks);
|
sortedTasks.sort((t1, t2) -> Integer.compare(t2.getPriority(), t1.getPriority()));
|
|
// 分批处理
|
List<List<ExecutingTask>> batches = createBatches(sortedTasks, batchSize);
|
|
for (int batchIndex = 0; batchIndex < batches.size(); batchIndex++) {
|
List<ExecutingTask> batch = batches.get(batchIndex);
|
|
System.out.println("处理批次 " + (batchIndex + 1) + "/" + batches.size() +
|
", 任务数: " + batch.size());
|
|
List<Future<PlannedPath>> futures = new ArrayList<>();
|
|
for (ExecutingTask task : batch) {
|
Future<PlannedPath> future = executorService.submit(() -> {
|
return planSingleTaskPath(task, constraints, spaceTimeOccupancyMap, agvStatusList, unifiedTimestamp);
|
});
|
futures.add(future);
|
}
|
|
for (Future<PlannedPath> future : futures) {
|
try {
|
PlannedPath path = future.get(60, TimeUnit.SECONDS);
|
if (path != null) {
|
allPaths.add(path);
|
System.out.println("CTU " + path.getAgvId() + " 路径规划成功");
|
}
|
} catch (TimeoutException e) {
|
System.out.println("路径规划超时，跳过");
|
future.cancel(true);
|
} catch (Exception e) {
|
System.out.println("路径规划异常: " + e.getMessage());
|
}
|
}
|
}
|
|
return allPaths;
|
}
|
|
/**
|
* 为单个任务规划路径
|
*/
|
private PlannedPath planSingleTaskPath(ExecutingTask task,
|
List<double[]> constraints,
|
Map<String, String> spaceTimeOccupancyMap,
|
List<AGVStatus> agvStatusList,
|
long unifiedTimestamp) {
|
|
String agvId = task.getAgvId();
|
String currentPos = task.getCurrentPosition();
|
String targetPos = task.getTargetPosition();
|
|
if (currentPos == null || targetPos == null) {
|
return null;
|
}
|
|
// 获取CTU状态信息
|
AGVStatus agvStatus = findAgvStatus(agvId, agvStatusList);
|
if (agvStatus == null) {
|
return null;
|
}
|
|
// 规划时空安全的路径（使用统一时间戳）
|
PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
|
currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
|
);
|
|
if (plannedPath != null) {
|
// 设置CTU信息
|
plannedPath.setAgvId(agvId);
|
plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));
|
|
enhancePathCodes(plannedPath, task);
|
}
|
|
return plannedPath;
|
}
|
|
/**
|
* 创建批次
|
*/
|
private List<List<ExecutingTask>> createBatches(List<ExecutingTask> tasks, int batchSize) {
|
List<List<ExecutingTask>> batches = new ArrayList<>();
|
|
for (int i = 0; i < tasks.size(); i += batchSize) {
|
int end = Math.min(i + batchSize, tasks.size());
|
batches.add(new ArrayList<>(tasks.subList(i, end)));
|
}
|
|
return batches;
|
}
|
|
/**
|
* 关闭服务
|
*/
|
public void shutdown() {
|
if (executorService != null && !executorService.isShutdown()) {
|
executorService.shutdown();
|
try {
|
if (!executorService.awaitTermination(10, TimeUnit.SECONDS)) {
|
executorService.shutdownNow();
|
}
|
} catch (InterruptedException e) {
|
executorService.shutdownNow();
|
Thread.currentThread().interrupt();
|
}
|
}
|
}
|
}
|
