package com.algo.service;
|
|
import com.algo.model.Conflict;
|
import com.algo.model.ExecutingTask;
|
import com.algo.model.PathCode;
|
import com.algo.model.PlannedPath;
|
import com.algo.model.CTUPhysicalConfig;
|
import com.algo.util.PathTimeCalculator;
|
|
import java.util.*;
|
|
/**
|
* 碰撞解决器
|
* 用于解决AGV路径之间的冲突
|
*/
|
public class CollisionResolver {
|
|
/**
|
* 碰撞检测器
|
*/
|
private final CollisionDetector collisionDetector;
|
|
/**
|
* 路径时间计算器
|
*/
|
private PathTimeCalculator timeCalculator;
|
|
/**
|
* 构造函数
|
*
|
* @param collisionDetector 碰撞检测器
|
*/
|
public CollisionResolver(CollisionDetector collisionDetector) {
|
this.collisionDetector = collisionDetector;
|
}
|
|
/**
|
* 设置时间计算器
|
*/
|
public void setTimeCalculator(PathTimeCalculator timeCalculator) {
|
this.timeCalculator = timeCalculator;
|
}
|
|
/**
|
* 解决路径冲突
|
*
|
* @param plannedPaths 规划路径列表
|
* @param conflicts 冲突列表
|
* @param executingTasks 执行中任务列表
|
* @return 解决冲突后的路径列表
|
*/
|
public List<PlannedPath> resolveConflicts(List<PlannedPath> plannedPaths,
|
List<Conflict> conflicts,
|
List<ExecutingTask> executingTasks) {
|
if (conflicts == null || conflicts.isEmpty()) {
|
return plannedPaths;
|
}
|
|
System.out.println("开始解决 " + conflicts.size() + " 个路径冲突");
|
|
// 构建路径字典便于快速访问
|
Map<String, PlannedPath> pathsMap = new HashMap<>();
|
for (PlannedPath path : plannedPaths) {
|
pathsMap.put(path.getAgvId(), path);
|
}
|
|
// 按时间步排序处理冲突
|
List<Conflict> sortedConflicts = new ArrayList<>(conflicts);
|
sortedConflicts.sort(Comparator.comparingInt(Conflict::getTimeStep));
|
|
// 逐个解决冲突
|
for (Conflict conflict : sortedConflicts) {
|
resolveSingleConflict(pathsMap, conflict, executingTasks);
|
}
|
|
List<PlannedPath> resolvedPaths = new ArrayList<>(pathsMap.values());
|
|
// 验证解决结果
|
List<Conflict> remainingConflicts = collisionDetector.detectConflicts(resolvedPaths);
|
System.out.println("冲突解决完成，剩余冲突数量: " + remainingConflicts.size());
|
|
return resolvedPaths;
|
}
|
|
/**
|
* 解决单个冲突
|
*
|
* @param pathsMap 路径映射
|
* @param conflict 冲突
|
* @param executingTasks 执行中任务列表
|
*/
|
private void resolveSingleConflict(Map<String, PlannedPath> pathsMap,
|
Conflict conflict,
|
List<ExecutingTask> executingTasks) {
|
String conflictType = conflict.getType();
|
|
switch (conflictType) {
|
case "vertex":
|
resolveVertexConflict(pathsMap, conflict, executingTasks);
|
break;
|
case "edge":
|
resolveEdgeConflict(pathsMap, conflict, executingTasks);
|
break;
|
case "follow":
|
resolveFollowingConflict(pathsMap, conflict, executingTasks);
|
break;
|
default:
|
System.out.println("未知冲突类型: " + conflictType);
|
}
|
}
|
|
/**
|
* 解决顶点冲突
|
*
|
* @param pathsMap 路径映射
|
* @param conflict 冲突
|
* @param executingTasks 执行中任务列表
|
*/
|
private void resolveVertexConflict(Map<String, PlannedPath> pathsMap,
|
Conflict conflict,
|
List<ExecutingTask> executingTasks) {
|
String agv1 = conflict.getAgv1();
|
String agv2 = conflict.getAgv2();
|
int timeStep = conflict.getTimeStep();
|
|
// 评估AGV优先级
|
AGVPriority priority1 = evaluateAgvPriority(agv1, pathsMap.get(agv1), executingTasks);
|
AGVPriority priority2 = evaluateAgvPriority(agv2, pathsMap.get(agv2), executingTasks);
|
|
// 优先级低的AGV进行延迟
|
if (priority1.priorityScore >= priority2.priorityScore) {
|
// agv2优先级低，延迟agv2
|
addDelayToPath(pathsMap.get(agv2), timeStep, 1);
|
System.out.println("为解决顶点冲突，AGV " + agv2 + " 在时间步 " + timeStep + " 延迟1步");
|
} else {
|
// agv1优先级低，延迟agv1
|
addDelayToPath(pathsMap.get(agv1), timeStep, 1);
|
System.out.println("为解决顶点冲突，AGV " + agv1 + " 在时间步 " + timeStep + " 延迟1步");
|
}
|
}
|
|
/**
|
* 解决边冲突
|
*
|
* @param pathsMap 路径映射
|
* @param conflict 冲突
|
* @param executingTasks 执行中任务列表
|
*/
|
private void resolveEdgeConflict(Map<String, PlannedPath> pathsMap,
|
Conflict conflict,
|
List<ExecutingTask> executingTasks) {
|
String agv1 = conflict.getAgv1();
|
String agv2 = conflict.getAgv2();
|
int timeStep = conflict.getTimeStep();
|
|
// 评估AGV优先级
|
AGVPriority priority1 = evaluateAgvPriority(agv1, pathsMap.get(agv1), executingTasks);
|
AGVPriority priority2 = evaluateAgvPriority(agv2, pathsMap.get(agv2), executingTasks);
|
|
// 优先级低的AGV进行延迟
|
if (priority1.priorityScore >= priority2.priorityScore) {
|
// agv2优先级低，延迟agv2
|
addDelayToPath(pathsMap.get(agv2), timeStep, 2);
|
System.out.println("为解决边冲突，AGV " + agv2 + " 在时间步 " + timeStep + " 延迟2步");
|
} else {
|
// agv1优先级低，延迟agv1
|
addDelayToPath(pathsMap.get(agv1), timeStep, 2);
|
System.out.println("为解决边冲突，AGV " + agv1 + " 在时间步 " + timeStep + " 延迟2步");
|
}
|
}
|
|
/**
|
* 解决跟随冲突
|
*
|
* @param pathsMap 路径映射
|
* @param conflict 冲突
|
* @param executingTasks 执行中任务列表
|
*/
|
private void resolveFollowingConflict(Map<String, PlannedPath> pathsMap,
|
Conflict conflict,
|
List<ExecutingTask> executingTasks) {
|
String agv1 = conflict.getAgv1();
|
String agv2 = conflict.getAgv2();
|
int timeStep = conflict.getTimeStep();
|
|
// 评估AGV优先级
|
AGVPriority priority1 = evaluateAgvPriority(agv1, pathsMap.get(agv1), executingTasks);
|
AGVPriority priority2 = evaluateAgvPriority(agv2, pathsMap.get(agv2), executingTasks);
|
|
// 优先级低的AGV进行延迟
|
if (priority1.priorityScore >= priority2.priorityScore) {
|
// agv2优先级低，延迟agv2
|
addDelayToPath(pathsMap.get(agv2), timeStep, 1);
|
System.out.println("为解决跟随冲突，AGV " + agv2 + " 在时间步 " + timeStep + " 延迟1步");
|
} else {
|
// agv1优先级低，延迟agv1
|
addDelayToPath(pathsMap.get(agv1), timeStep, 1);
|
System.out.println("为解决跟随冲突，AGV " + agv1 + " 在时间步 " + timeStep + " 延迟1步");
|
}
|
}
|
|
/**
|
* 评估AGV优先级 - 优化版本，只保留必要字段
|
*
|
* @param agvId AGV编号
|
* @param path 路径
|
* @param executingTasks 执行中任务列表
|
* @return AGV优先级信息
|
*/
|
private AGVPriority evaluateAgvPriority(String agvId, PlannedPath path, List<ExecutingTask> executingTasks) {
|
// 查找对应的执行任务
|
ExecutingTask task = null;
|
for (ExecutingTask et : executingTasks) {
|
if (et.getAgvId().equals(agvId)) {
|
task = et;
|
break;
|
}
|
}
|
|
double priorityScore = 0.0;
|
String explanation = "默认优先级";
|
|
if (task != null) {
|
String taskStatus = task.getTaskType();
|
int taskPriority = task.getPriority();
|
|
// 基于任务类型的优先级评分
|
switch (taskStatus) {
|
case "delivery":
|
priorityScore += 50; // 送货任务优先级高
|
explanation = "送货任务 - 高优先级";
|
break;
|
case "pickup":
|
priorityScore += 30; // 取货任务中等优先级
|
explanation = "取货任务 - 中等优先级";
|
break;
|
case "charging":
|
priorityScore += 20; // 充电任务低优先级
|
explanation = "充电任务 - 低优先级";
|
break;
|
default:
|
priorityScore += 10; // 其他任务最低优先级
|
explanation = "其他任务 - 最低优先级";
|
}
|
|
// 任务优先级加成
|
priorityScore += taskPriority * 10;
|
|
// 路径长度因子（路径越短优先级越高）
|
if (path != null && path.getCodeList() != null) {
|
double pathLengthFactor = Math.max(0, 50 - path.getCodeList().size());
|
priorityScore += pathLengthFactor;
|
}
|
}
|
|
return new AGVPriority(agvId, priorityScore, explanation);
|
}
|
|
/**
|
* 为路径添加延迟（增强版，正确重计算时间窗）
|
*
|
* @param path 路径
|
* @param timeStep 延迟开始的时间步
|
* @param delaySteps 延迟步数
|
*/
|
private void addDelayToPath(PlannedPath path, int timeStep, int delaySteps) {
|
if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
|
return;
|
}
|
|
List<PathCode> codeList = path.getCodeList();
|
|
// 确保时间步在有效范围内
|
if (timeStep < 0 || timeStep >= codeList.size()) {
|
return;
|
}
|
|
// 获取延迟位置的路径代码
|
PathCode delayCode = codeList.get(timeStep);
|
|
// 计算延迟时间（每个延迟步骤1秒）
|
long delayDuration = delaySteps * 1000L;
|
|
// 创建延迟步骤
|
List<PathCode> delaySteps_list = new ArrayList<>();
|
for (int i = 0; i < delaySteps; i++) {
|
PathCode waitCode = new PathCode(delayCode.getCode(), delayCode.getDirection());
|
waitCode.setActionType(delayCode.getActionType());
|
waitCode.setTaskId(delayCode.getTaskId());
|
waitCode.setPosType(delayCode.getPosType());
|
waitCode.setLev(delayCode.getLev());
|
waitCode.setTargetPoint(false); // 等待步骤不是目标点
|
|
// 设置等待步骤的时间信息
|
if (delayCode.getArrivalTime() != null) {
|
long waitStartTime = delayCode.getArrivalTime() + (i * 1000L);
|
waitCode.setArrivalTime(waitStartTime);
|
waitCode.setDepartureTime(waitStartTime + 1000L);
|
}
|
|
delaySteps_list.add(waitCode);
|
}
|
|
// 插入延迟步骤
|
codeList.addAll(timeStep, delaySteps_list);
|
|
// 更新原路径点的时间（向后推迟）
|
if (delayCode.getArrivalTime() != null) {
|
delayCode.setArrivalTime(delayCode.getArrivalTime() + delayDuration);
|
if (delayCode.getDepartureTime() != null) {
|
delayCode.setDepartureTime(delayCode.getDepartureTime() + delayDuration);
|
}
|
}
|
|
// 更新路径
|
path.setCodeList(codeList);
|
|
// 🔧 关键修复：重新计算后续路径点的时间窗
|
if (timeCalculator != null) {
|
CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
|
timeCalculator.recalculatePathTimingFromIndex(path, timeStep + delaySteps, defaultConfig);
|
} else {
|
// 备用方案：手动更新后续时间
|
updateSubsequentPathTiming(path, timeStep + delaySteps, delayDuration);
|
}
|
}
|
|
/**
|
* 备用方案：手动更新后续路径点的时间
|
*/
|
private void updateSubsequentPathTiming(PlannedPath path, int fromIndex, long timeOffset) {
|
List<PathCode> codeList = path.getCodeList();
|
|
for (int i = fromIndex; i < codeList.size(); i++) {
|
PathCode pathCode = codeList.get(i);
|
|
if (pathCode.getArrivalTime() != null) {
|
pathCode.setArrivalTime(pathCode.getArrivalTime() + timeOffset);
|
}
|
|
if (pathCode.getDepartureTime() != null) {
|
pathCode.setDepartureTime(pathCode.getDepartureTime() + timeOffset);
|
}
|
|
if (pathCode.getCumulativeTime() != null) {
|
pathCode.setCumulativeTime(pathCode.getCumulativeTime() + timeOffset);
|
}
|
}
|
}
|
|
/**
|
* 创建默认物理配置
|
*/
|
private CTUPhysicalConfig createDefaultPhysicalConfig() {
|
return new CTUPhysicalConfig(); // 使用默认构造函数
|
}
|
|
/**
|
* AGV优先级评估结果内部类 - 优化版本，只保留必要字段
|
*/
|
private static class AGVPriority {
|
final String agvId;
|
final double priorityScore;
|
final String explanation;
|
|
public AGVPriority(String agvId, double priorityScore, String explanation) {
|
this.agvId = agvId;
|
this.priorityScore = priorityScore;
|
this.explanation = explanation;
|
}
|
|
@Override
|
public String toString() {
|
return String.format("AGV %s: 优先级=%.1f, 说明=%s",
|
agvId, priorityScore, explanation);
|
}
|
}
|
}
|
