| algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| algo-zkd/src/main/java/com/algo/service/CollisionDetector.java | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| algo-zkd/src/main/java/com/algo/service/CollisionResolver.java | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| algo-zkd/src/main/java/com/algo/service/ExecutingTaskExtractor.java | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
| algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 |
algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
@@ -268,7 +268,7 @@ gScores.put(startKey, 0.0); // 构建约束检查器 EnhancedConstraintChecker constraintChecker = new EnhancedConstraintChecker( ConstraintChecker constraintChecker = new ConstraintChecker( constraints, occupancyMap, pathMapping, physicalConfig ); @@ -315,7 +315,7 @@ PriorityQueue<SpaceTimeAStarNode> openSet, Set<String> closedSet, Map<String, Double> gScores, Map<String, SpaceTimeAStarNode> cameFrom, EnhancedConstraintChecker constraintChecker, ConstraintChecker constraintChecker, CTUPhysicalConfig physicalConfig) { // 获取空间邻居 @@ -399,7 +399,7 @@ * 检查当前节点的邻居是否在短期内都被阻挡 */ private boolean checkIfAllNeighborsBlocked(SpaceTimeAStarNode current, EnhancedConstraintChecker constraintChecker, ConstraintChecker constraintChecker, CTUPhysicalConfig physicalConfig) { // 获取当前节点的所有空间邻居 List<Map<String, String>> neighbors = getNeighbors(current.code); @@ -872,16 +872,16 @@ /** * 约束检查器 */ private static class EnhancedConstraintChecker { private static class ConstraintChecker { private final List<double[]> staticConstraints; private final Map<String, String> spaceTimeOccupancyMap; private final Map<String, Map<String, Integer>> pathMapping; private final CTUPhysicalConfig physicalConfig; public EnhancedConstraintChecker(List<double[]> staticConstraints, Map<String, String> spaceTimeOccupancyMap, Map<String, Map<String, Integer>> pathMapping, CTUPhysicalConfig physicalConfig) { public ConstraintChecker(List<double[]> staticConstraints, Map<String, String> spaceTimeOccupancyMap, Map<String, Map<String, Integer>> pathMapping, CTUPhysicalConfig physicalConfig) { this.staticConstraints = staticConstraints; this.spaceTimeOccupancyMap = spaceTimeOccupancyMap != null ? spaceTimeOccupancyMap : new HashMap<>(); this.pathMapping = pathMapping; algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
@@ -70,21 +70,21 @@ System.out.println("开始碰撞检测，路径数量: " + plannedPaths.size()); // 构建高精度时空表 Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = buildEnhancedSpaceTimeTable(plannedPaths); // 构建时空表 Map<Long, List<SpaceTimeNode>> spaceTimeTable = buildSpaceTimeTable(plannedPaths); // 检测顶点冲突（同一时间同一位置） List<Conflict> vertexConflicts = detectEnhancedVertexConflicts(spaceTimeTable); List<Conflict> vertexConflicts = detectVertexConflicts(spaceTimeTable); conflicts.addAll(vertexConflicts); System.out.println("顶点冲突数量: " + vertexConflicts.size()); // 检测边冲突（CTU交换位置） List<Conflict> edgeConflicts = detectEnhancedEdgeConflicts(spaceTimeTable); List<Conflict> edgeConflicts = detectEdgeConflicts(spaceTimeTable); conflicts.addAll(edgeConflicts); System.out.println("边冲突数量: " + edgeConflicts.size()); // 检测跟随冲突 List<Conflict> followingConflicts = detectEnhancedFollowingConflicts(spaceTimeTable); List<Conflict> followingConflicts = detectFollowingConflicts(spaceTimeTable); conflicts.addAll(followingConflicts); System.out.println("跟随冲突数量: " + followingConflicts.size()); @@ -103,8 +103,8 @@ * @param plannedPaths 规划路径列表 * @return 时空表 */ private Map<Long, List<EnhancedSpaceTimeNode>> buildEnhancedSpaceTimeTable(List<PlannedPath> plannedPaths) { Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = new HashMap<>(); private Map<Long, List<SpaceTimeNode>> buildSpaceTimeTable(List<PlannedPath> plannedPaths) { Map<Long, List<SpaceTimeNode>> spaceTimeTable = new HashMap<>(); for (PlannedPath path : plannedPaths) { String agvId = path.getAgvId(); @@ -133,7 +133,7 @@ if (coordinates != null) { // 创建时间段内的多个时间点（使用实际时间戳） for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) { EnhancedSpaceTimeNode node = new EnhancedSpaceTimeNode( SpaceTimeNode node = new SpaceTimeNode( agvId, position, coordinates, timePoint, arrivalTime, departureTime, physicalConfig ); @@ -261,36 +261,36 @@ * @param spaceTimeTable 时空表 * @return 顶点冲突列表 */ private List<Conflict> detectEnhancedVertexConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) { private List<Conflict> detectVertexConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) { List<Conflict> conflicts = new ArrayList<>(); for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) { for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) { long timePoint = entry.getKey(); List<EnhancedSpaceTimeNode> nodes = entry.getValue(); List<SpaceTimeNode> nodes = entry.getValue(); // 按位置分组 Map<String, List<EnhancedSpaceTimeNode>> positionGroups = new HashMap<>(); for (EnhancedSpaceTimeNode node : nodes) { Map<String, List<SpaceTimeNode>> positionGroups = new HashMap<>(); for (SpaceTimeNode node : nodes) { positionGroups.computeIfAbsent(node.position, k -> new ArrayList<>()).add(node); } // 检查每个位置是否有多个CTU for (Map.Entry<String, List<EnhancedSpaceTimeNode>> posEntry : positionGroups.entrySet()) { for (Map.Entry<String, List<SpaceTimeNode>> posEntry : positionGroups.entrySet()) { String position = posEntry.getKey(); List<EnhancedSpaceTimeNode> ctuNodes = posEntry.getValue(); List<SpaceTimeNode> ctuNodes = posEntry.getValue(); if (ctuNodes.size() > 1) { // 进一步检查时间重叠 for (int i = 0; i < ctuNodes.size(); i++) { for (int j = i + 1; j < ctuNodes.size(); j++) { EnhancedSpaceTimeNode node1 = ctuNodes.get(i); EnhancedSpaceTimeNode node2 = ctuNodes.get(j); SpaceTimeNode node1 = ctuNodes.get(i); SpaceTimeNode node2 = ctuNodes.get(j); // 检查时间段是否重叠 if (timeRangeOverlap(node1.arrivalTime, node1.departureTime, node2.arrivalTime, node2.departureTime)) { Conflict conflict = new Conflict( "enhanced_vertex", "vertex", node1.agvId, node2.agvId, (int) (timePoint / 1000), // 转换为秒用于显示 @@ -316,7 +316,7 @@ * @param spaceTimeTable 时空表 * @return 边冲突列表 */ private List<Conflict> detectEnhancedEdgeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) { private List<Conflict> detectEdgeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) { List<Conflict> conflicts = new ArrayList<>(); if (spaceTimeTable.isEmpty()) { @@ -332,8 +332,8 @@ // 只检查连续的时间点 if (nextTime - currentTime <= timeResolution * 2) { List<EnhancedSpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime); List<EnhancedSpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime); List<SpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime); List<SpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime); conflicts.addAll(detectPositionSwapping(currentNodes, nextNodes, currentTime, nextTime)); } @@ -351,19 +351,19 @@ * @param nextTime 下一时间 * @return 冲突列表 */ private List<Conflict> detectPositionSwapping(List<EnhancedSpaceTimeNode> currentNodes, List<EnhancedSpaceTimeNode> nextNodes, private List<Conflict> detectPositionSwapping(List<SpaceTimeNode> currentNodes, List<SpaceTimeNode> nextNodes, long currentTime, long nextTime) { List<Conflict> conflicts = new ArrayList<>(); Map<String, String> currentPositions = new HashMap<>(); Map<String, String> nextPositions = new HashMap<>(); for (EnhancedSpaceTimeNode node : currentNodes) { for (SpaceTimeNode node : currentNodes) { currentPositions.put(node.agvId, node.position); } for (EnhancedSpaceTimeNode node : nextNodes) { for (SpaceTimeNode node : nextNodes) { nextPositions.put(node.agvId, node.position); } @@ -386,7 +386,7 @@ pos1Current.equals(pos2Next) && pos2Current.equals(pos1Next)) { Conflict conflict = new Conflict( "enhanced_edge", "edge", ctu1, ctu2, (int) (currentTime / 1000), @@ -409,17 +409,18 @@ * @param spaceTimeTable 时空表 * @return 跟随冲突列表 */ private List<Conflict> detectEnhancedFollowingConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) { private List<Conflict> detectFollowingConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) { List<Conflict> conflicts = new ArrayList<>(); for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) { // 第一部分：检查同一时间点的节点 for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) { long timePoint = entry.getKey(); List<EnhancedSpaceTimeNode> nodes = entry.getValue(); List<SpaceTimeNode> nodes = entry.getValue(); for (int i = 0; i < nodes.size(); i++) { for (int j = i + 1; j < nodes.size(); j++) { EnhancedSpaceTimeNode node1 = nodes.get(i); EnhancedSpaceTimeNode node2 = nodes.get(j); SpaceTimeNode node1 = nodes.get(i); SpaceTimeNode node2 = nodes.get(j); double distance = calculateDistance(node1.coordinates, node2.coordinates); double minSafeDistance = Math.max( @@ -429,7 +430,7 @@ if (distance > 0 && distance < minSafeDistance) { Conflict conflict = new Conflict( "enhanced_follow", "follow", node1.agvId, node2.agvId, (int) (timePoint / 1000), @@ -441,22 +442,18 @@ conflicts.add(conflict); } // 检查同一位置或相邻位置的时间间隔是否足够 // 如果两个AGV在同一位置或相邻位置，要求最小时间间隔 if (distance <= minSafeDistance) { // 检查同一时间点的时间范围重叠（同时占用） if (distance <= minSafeDistance && timeRangeOverlap( node1.arrivalTime, node1.departureTime, node2.arrivalTime, node2.departureTime)) { double normalSpeed = Math.min( node1.physicalConfig.getNormalSpeed(), node2.physicalConfig.getNormalSpeed() ); // 最小时间间隔 = 安全距离/速度 + 额外安全缓冲(10秒) // 总时间间隔约12~13秒，用于防止计算误差 long minTimeGap = (long) ((minSafeDistance / normalSpeed + 10.0) * 1000); // 检查时间间隔 long timeGap = Math.abs(node1.arrivalTime - node2.arrivalTime); if (timeGap < minTimeGap && timeRangeOverlap( node1.arrivalTime, node1.departureTime, node2.arrivalTime, node2.departureTime)) { if (timeGap < minTimeGap) { Conflict conflict = new Conflict( "time_gap_insufficient", node1.agvId, @@ -464,12 +461,104 @@ (int) (timePoint / 1000), node1.position, node2.position, String.format("CTU %s 和 %s 在位置 %s/%s 时间间隔不足 (%.2f秒 < %.2f秒)", String.format("CTU %s 和 %s 在位置 %s/%s 同时占用且时间间隔不足 (%.2f秒 < %.2f秒)", node1.agvId, node2.agvId, node1.position, node2.position, timeGap / 1000.0, minTimeGap / 1000.0) ); conflicts.add(conflict); } } } } } // 第二部分：检查同一位置连续经过的时间间隔 Map<String, Map<String, SpaceTimeNode>> positionAgvMap = new HashMap<>(); for (List<SpaceTimeNode> nodes : spaceTimeTable.values()) { for (SpaceTimeNode node : nodes) { positionAgvMap.computeIfAbsent(node.position, k -> new HashMap<>()) .compute(node.agvId, (k, existing) -> { if (existing == null) { return node; } else { // 合并时间范围 long minArrival = Math.min(existing.arrivalTime, node.arrivalTime); long maxDeparture = Math.max(existing.departureTime, node.departureTime); return new SpaceTimeNode( node.agvId, node.position, node.coordinates, minArrival, minArrival, maxDeparture, node.physicalConfig ); } }); } } // 对于每个位置，检查所有经过该位置的AGV的时间间隔 Set<String> processedPairs = new HashSet<>(); for (Map.Entry<String, Map<String, SpaceTimeNode>> positionEntry : positionAgvMap.entrySet()) { String position = positionEntry.getKey(); Map<String, SpaceTimeNode> agvNodes = positionEntry.getValue(); List<SpaceTimeNode> nodes = new ArrayList<>(agvNodes.values()); nodes.sort((n1, n2) -> Long.compare(n1.arrivalTime, n2.arrivalTime)); for (int i = 0; i < nodes.size(); i++) { for (int j = i + 1; j < nodes.size(); j++) { SpaceTimeNode node1 = nodes.get(i); SpaceTimeNode node2 = nodes.get(j); if (node1.agvId.equals(node2.agvId)) { continue; } String pairKey1 = node1.agvId + "_" + node2.agvId + "_" + position; String pairKey2 = node2.agvId + "_" + node1.agvId + "_" + position; if (processedPairs.contains(pairKey1) || processedPairs.contains(pairKey2)) { continue; } // 计算时间间隔：node1离开后，node2到达的时间间隔 long timeGap; if (node1.departureTime <= node2.arrivalTime) { timeGap = node2.arrivalTime - node1.departureTime; } else if (node2.departureTime <= node1.arrivalTime) { timeGap = node1.arrivalTime - node2.departureTime; } else { continue; } // 计算最小安全时间间隔 double minSafeDistance = Math.max( node1.physicalConfig.getMinSafetyDistance(), node2.physicalConfig.getMinSafetyDistance() ); double normalSpeed = Math.min( node1.physicalConfig.getNormalSpeed(), node2.physicalConfig.getNormalSpeed() ); // 最小时间间隔 = 安全距离/速度 + 额外安全缓冲(7秒) long minTimeGap = (long) ((minSafeDistance / normalSpeed + 7.0) * 1000); // 检查时间间隔是否足够 if (timeGap < minTimeGap) { String firstAgv = node1.arrivalTime < node2.arrivalTime ? node1.agvId : node2.agvId; String secondAgv = node1.arrivalTime < node2.arrivalTime ? node2.agvId : node1.agvId; long secondArrival = node1.arrivalTime < node2.arrivalTime ? node2.arrivalTime : node1.arrivalTime; Conflict conflict = new Conflict( "time_gap_insufficient", firstAgv, secondAgv, (int) (secondArrival / 1000), position, position, String.format("CTU %s 和 %s 在位置 %s 连续经过时间间隔不足 (%.2f秒 < %.2f秒, %s离开后%.2f秒%s到达)", firstAgv, secondAgv, position, timeGap / 1000.0, minTimeGap / 1000.0, firstAgv, timeGap / 1000.0, secondAgv) ); conflicts.add(conflict); processedPairs.add(pairKey1); } } } @@ -484,17 +573,17 @@ * @param spaceTimeTable 时空表 * @return 物理冲突列表 */ private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) { private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) { List<Conflict> conflicts = new ArrayList<>(); for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) { for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) { long timePoint = entry.getKey(); List<EnhancedSpaceTimeNode> nodes = entry.getValue(); List<SpaceTimeNode> nodes = entry.getValue(); for (int i = 0; i < nodes.size(); i++) { for (int j = i + 1; j < nodes.size(); j++) { EnhancedSpaceTimeNode node1 = nodes.get(i); EnhancedSpaceTimeNode node2 = nodes.get(j); SpaceTimeNode node1 = nodes.get(i); SpaceTimeNode node2 = nodes.get(j); if (checkPhysicalCollision(node1, node2)) { Conflict conflict = new Conflict( @@ -523,7 +612,7 @@ * @param node2 CTU节点2 * @return 是否碰撞 */ private boolean checkPhysicalCollision(EnhancedSpaceTimeNode node1, EnhancedSpaceTimeNode node2) { private boolean checkPhysicalCollision(SpaceTimeNode node1, SpaceTimeNode node2) { double distance = calculateDistance(node1.coordinates, node2.coordinates); // 考虑CTU的物理尺寸 @@ -566,7 +655,7 @@ /** * 时空节点内部类 */ private static class EnhancedSpaceTimeNode { private static class SpaceTimeNode { final String agvId; final String position; final int[] coordinates; @@ -575,9 +664,9 @@ final long departureTime; final CTUPhysicalConfig physicalConfig; public EnhancedSpaceTimeNode(String agvId, String position, int[] coordinates, long timePoint, long arrivalTime, long departureTime, CTUPhysicalConfig physicalConfig) { public SpaceTimeNode(String agvId, String position, int[] coordinates, long timePoint, long arrivalTime, long departureTime, CTUPhysicalConfig physicalConfig) { this.agvId = agvId; this.position = position; this.coordinates = coordinates; algo-zkd/src/main/java/com/algo/service/CollisionResolver.java
@@ -1,10 +1,12 @@ package com.algo.service; import com.algo.model.AGVStatus; 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.JsonUtils; import com.algo.util.PathTimeCalculator; import java.util.*; @@ -24,6 +26,16 @@ * 路径时间计算器 */ private PathTimeCalculator timeCalculator; /** * 路径规划器 */ private PathPlanner pathPlanner; /** * 路径映射表（用于坐标转换） */ private Map<String, Map<String, Integer>> pathMapping; /** * 构造函数 @@ -40,6 +52,20 @@ public void setTimeCalculator(PathTimeCalculator timeCalculator) { this.timeCalculator = timeCalculator; } /** * 设置路径规划器 */ public void setPathPlanner(PathPlanner pathPlanner) { this.pathPlanner = pathPlanner; } /** * 设置路径映射表 */ public void setPathMapping(Map<String, Map<String, Integer>> pathMapping) { this.pathMapping = pathMapping; } /** * 解决路径冲突 @@ -47,11 +73,17 @@ * @param plannedPaths 规划路径列表 * @param conflicts 冲突列表 * @param executingTasks 执行中任务列表 * @param agvStatusList AGV状态列表（用于判断剩余路径） * @return 解决冲突后的路径列表 */ /** * 解决路径冲突 */ public List<PlannedPath> resolveConflicts(List<PlannedPath> plannedPaths, List<Conflict> conflicts, List<ExecutingTask> executingTasks) { List<ExecutingTask> executingTasks, List<AGVStatus> agvStatusList, long unifiedTimestamp) { if (conflicts == null || conflicts.isEmpty()) { return plannedPaths; } @@ -63,6 +95,28 @@ for (PlannedPath path : plannedPaths) { pathsMap.put(path.getAgvId(), path); } // 构建AGV状态字典 Map<String, AGVStatus> agvStatusMap = new HashMap<>(); if (agvStatusList != null) { for (AGVStatus agv : agvStatusList) { if (agv.getAgvId() != null) { agvStatusMap.put(agv.getAgvId(), agv); } } } // 构建所有已规划路径的占用位置集合（用于查找等待位置时避开） Set<String> allOccupiedPositions = new HashSet<>(); for (PlannedPath path : plannedPaths) { if (path.getCodeList() != null) { for (PathCode code : path.getCodeList()) { if (code.getCode() != null) { allOccupiedPositions.add(code.getCode()); } } } } // 按时间步排序处理冲突 List<Conflict> sortedConflicts = new ArrayList<>(conflicts); @@ -70,7 +124,9 @@ // 逐个解决冲突 for (Conflict conflict : sortedConflicts) { resolveSingleConflict(pathsMap, conflict, executingTasks); // 对于每个冲突，计算两车路径重合的位置 Set<String> overlappingPositions = findOverlappingPositions(pathsMap, conflict); resolveSingleConflict(pathsMap, conflict, executingTasks, agvStatusMap, overlappingPositions, unifiedTimestamp); } List<PlannedPath> resolvedPaths = new ArrayList<>(pathsMap.values()); @@ -81,17 +137,30 @@ return resolvedPaths; } /** * 解决路径冲突 * * @param plannedPaths 规划路径列表 * @param conflicts 冲突列表 * @param executingTasks 执行中任务列表 * @return 解决冲突后的路径列表 */ public List<PlannedPath> resolveConflicts(List<PlannedPath> plannedPaths, List<Conflict> conflicts, List<ExecutingTask> executingTasks) { return resolveConflicts(plannedPaths, conflicts, executingTasks, null, System.currentTimeMillis() / 1000); } /** * 解决单个冲突 * * @param pathsMap 路径映射 * @param conflict 冲突 * @param executingTasks 执行中任务列表 */ private void resolveSingleConflict(Map<String, PlannedPath> pathsMap, Conflict conflict, List<ExecutingTask> executingTasks) { List<ExecutingTask> executingTasks, Map<String, AGVStatus> agvStatusMap, Set<String> overlappingPositions, long unifiedTimestamp) { String conflictType = conflict.getType(); switch (conflictType) { @@ -103,6 +172,9 @@ break; case "follow": resolveFollowingConflict(pathsMap, conflict, executingTasks); break; case "time_gap_insufficient": resolveTimeGapConflict(pathsMap, conflict, executingTasks, agvStatusMap, overlappingPositions, unifiedTimestamp); break; default: System.out.println("未知冲突类型: " + conflictType); @@ -177,8 +249,8 @@ * @param executingTasks 执行中任务列表 */ private void resolveFollowingConflict(Map<String, PlannedPath> pathsMap, Conflict conflict, List<ExecutingTask> executingTasks) { Conflict conflict, List<ExecutingTask> executingTasks) { String agv1 = conflict.getAgv1(); String agv2 = conflict.getAgv2(); int timeStep = conflict.getTimeStep(); @@ -200,7 +272,577 @@ } /** * 评估AGV优先级 - 优化版本，只保留必要字段 * 找到两车路径重合的位置 */ private Set<String> findOverlappingPositions(Map<String, PlannedPath> pathsMap, Conflict conflict) { Set<String> overlappingPositions = new HashSet<>(); PlannedPath path1 = pathsMap.get(conflict.getAgv1()); PlannedPath path2 = pathsMap.get(conflict.getAgv2()); if (path1 == null || path2 == null || path1.getCodeList() == null || path2.getCodeList() == null) { return overlappingPositions; } // 收集path1的所有位置 Set<String> positions1 = new HashSet<>(); for (PathCode code : path1.getCodeList()) { if (code.getCode() != null) { positions1.add(code.getCode()); } } // 收集path2的所有位置 Set<String> positions2 = new HashSet<>(); for (PathCode code : path2.getCodeList()) { if (code.getCode() != null) { positions2.add(code.getCode()); } } // 找到重合的位置 for (String pos : positions1) { if (positions2.contains(pos)) { overlappingPositions.add(pos); } } System.out.println("[调试] 两车路径重合位置: " + overlappingPositions); return overlappingPositions; } /** * 解决时间间隔不足冲突 * 策略：有剩余路径的AGV继续执行，新路径的AGV移动到安全等待位置 */ private void resolveTimeGapConflict(Map<String, PlannedPath> pathsMap, Conflict conflict, List<ExecutingTask> executingTasks, Map<String, AGVStatus> agvStatusMap, Set<String> overlappingPositions, long unifiedTimestamp) { String firstAgv = conflict.getAgv1(); // 先经过的AGV String secondAgv = conflict.getAgv2(); // 后经过的AGV String conflictPosition = conflict.getPosition1(); // 冲突位置 PlannedPath path1 = pathsMap.get(firstAgv); PlannedPath path2 = pathsMap.get(secondAgv); if (path1 == null || path2 == null) { System.out.println("警告: 无法解决冲突，路径不存在"); return; } // 打印原始路径信息（调试用） System.out.println("[冲突解决debug] 冲突位置: " + conflictPosition); System.out.println("[冲突解决debug] AGV " + firstAgv + " 原始路径: " + (path1.getCodeList() != null && !path1.getCodeList().isEmpty() ? path1.getCodeList().get(0).getCode() + " -> " + path1.getCodeList().get(path1.getCodeList().size() - 1).getCode() + " (共" + path1.getCodeList().size() + "点)" : "空路径")); System.out.println("[冲突解决debug] AGV " + secondAgv + " 原始路径: " + (path2.getCodeList() != null && !path2.getCodeList().isEmpty() ? path2.getCodeList().get(0).getCode() + " -> " + path2.getCodeList().get(path2.getCodeList().size() - 1).getCode() + " (共" + path2.getCodeList().size() + "点)" : "空路径")); // 检查原始路径是否经过冲突位置 if (path1.getCodeList() != null) { boolean path1HasConflictPos = path1.getCodeList().stream() .anyMatch(code -> conflictPosition.equals(code.getCode())); System.out.println("[冲突解决debug] AGV " + firstAgv + " 路径是否经过冲突位置 " + conflictPosition + ": " + path1HasConflictPos); } if (path2.getCodeList() != null) { boolean path2HasConflictPos = path2.getCodeList().stream() .anyMatch(code -> conflictPosition.equals(code.getCode())); System.out.println("[冲突解决debug] AGV " + secondAgv + " 路径是否经过冲突位置 " + conflictPosition + ": " + path2HasConflictPos); } // 判断哪个AGV有剩余路径 AGVStatus agv1Status = agvStatusMap != null ? agvStatusMap.get(firstAgv) : null; AGVStatus agv2Status = agvStatusMap != null ? agvStatusMap.get(secondAgv) : null; boolean agv1HasRemainingPath = agv1Status != null && agv1Status.hasRemainingPath(); boolean agv2HasRemainingPath = agv2Status != null && agv2Status.hasRemainingPath(); // 确定需要等待的AGV String waitingAgv; PlannedPath waitingPath; AGVStatus waitingAgvStatus; if (agv1HasRemainingPath && !agv2HasRemainingPath) { // AGV1有剩余路径，AGV2是新路径 -> AGV2等待 waitingAgv = secondAgv; waitingPath = path2; waitingAgvStatus = agv2Status; System.out.println("冲突解决: AGV " + firstAgv + " 有剩余路径继续执行，AGV " + secondAgv + " 新路径需要等待"); } else if (!agv1HasRemainingPath && agv2HasRemainingPath) { // AGV1是新路径，AGV2有剩余路径 -> AGV1等待 waitingAgv = firstAgv; waitingPath = path1; waitingAgvStatus = agv1Status; System.out.println("冲突解决: AGV " + secondAgv + " 有剩余路径继续执行，AGV " + firstAgv + " 新路径需要等待"); } else if (agv1HasRemainingPath && agv2HasRemainingPath) { // 两个都有剩余路径(最好不出现这个情况) AGVPriority priority1 = evaluateAgvPriority(firstAgv, path1, executingTasks); AGVPriority priority2 = evaluateAgvPriority(secondAgv, path2, executingTasks); if (priority1.priorityScore >= priority2.priorityScore) { // AGV1优先级高，AGV2等待 waitingAgv = secondAgv; waitingPath = path2; waitingAgvStatus = agv2Status; System.out.println("冲突解决: 两个都有剩余路径，AGV " + firstAgv + " 优先级高继续执行，AGV " + secondAgv + " 等待"); } else { // AGV2优先级高，AGV1等待 waitingAgv = firstAgv; waitingPath = path1; waitingAgvStatus = agv1Status; System.out.println("冲突解决: 两个都有剩余路径，AGV " + secondAgv + " 优先级高继续执行，AGV " + firstAgv + " 等待"); } } else { // 两个都是新路径 -> 根据优先级决定 AGVPriority priority1 = evaluateAgvPriority(firstAgv, path1, executingTasks); AGVPriority priority2 = evaluateAgvPriority(secondAgv, path2, executingTasks); if (priority1.priorityScore >= priority2.priorityScore) { waitingAgv = secondAgv; waitingPath = path2; waitingAgvStatus = agv2Status; System.out.println("冲突解决: 两个都是新路径，AGV " + firstAgv + " 优先级高继续执行，AGV " + secondAgv + " 等待"); } else { waitingAgv = firstAgv; waitingPath = path1; waitingAgvStatus = agv1Status; System.out.println("冲突解决: 两个都是新路径，AGV " + secondAgv + " 优先级高继续执行，AGV " + firstAgv + " 等待"); } } // 找到等待AGV的当前位置（路径起点） String currentPosition = null; if (waitingPath != null && !waitingPath.getCodeList().isEmpty()) { currentPosition = waitingPath.getCodeList().get(0).getCode(); } if (currentPosition == null && waitingAgvStatus != null) { currentPosition = waitingAgvStatus.getPosition(); } if (currentPosition == null || conflictPosition == null) { System.out.println("警告: 无法找到当前位置或冲突位置，跳过冲突解决"); return; } // 检查等待AGV的路径是否经过冲突位置 boolean waitingPathHasConflictPos = false; if (waitingPath != null && waitingPath.getCodeList() != null) { waitingPathHasConflictPos = waitingPath.getCodeList().stream() .anyMatch(code -> conflictPosition.equals(code.getCode())); } if (!waitingPathHasConflictPos) { System.out.println("AGV " + waitingAgv + " 的路径不经过冲突位置 " + conflictPosition + "，跳过冲突解决"); return; } String waitPosition = findWaitPositionOnOriginalPath(waitingPath, conflictPosition, waitingAgvStatus, overlappingPositions); boolean waitPositionOnOriginalPath = (waitPosition != null); if (waitPosition == null) { System.out.println("原始路径上没有合适的等待位置，尝试寻找其他位置"); waitPosition = findSafeWaitingPosition(currentPosition, conflictPosition, waitingAgvStatus, overlappingPositions); } if (waitPosition == null) { System.out.println("警告: 无法找到安全等待位置，AGV " + waitingAgv + " 保持当前位置"); // 如果找不到等待位置，至少截断路径，只保留到当前位置 truncatePathToCurrentPosition(waitingPath, currentPosition); return; } if (waitPositionOnOriginalPath && waitingPath != null) { truncatePathToWaitPosition(waitingPath, waitPosition); System.out.println("AGV " + waitingAgv + " 路径已截断到等待位置 " + waitPosition + "（在原始路径上，距离冲突点 " + conflictPosition + " 至少5步）"); return; } String originalSegId = waitingPath != null ? waitingPath.getSegId() : null; ExecutingTask waitingTask = null; if (executingTasks != null) { for (ExecutingTask task : executingTasks) { if (task.getAgvId().equals(waitingAgv)) { waitingTask = task; break; } } } // 规划到等待位置的路径 PlannedPath newWaitPath = planPathToWaitPosition(waitingAgv, currentPosition, waitPosition, waitingAgvStatus, originalSegId, waitingTask, unifiedTimestamp); if (newWaitPath != null) { pathsMap.put(waitingAgv, newWaitPath); System.out.println("AGV " + waitingAgv + " 已规划到等待位置 " + waitPosition + "（距离冲突点 " + conflictPosition + " 至少5步）"); } else { System.out.println("警告: 无法规划到等待位置的路径，AGV " + waitingAgv + " 保持当前位置"); truncatePathToCurrentPosition(waitingPath, currentPosition); } } /** * 找到距离冲突点安全距离的安全等待位置 */ private String findSafeWaitingPosition(String currentPosition, String conflictPosition, AGVStatus agvStatus, Set<String> overlappingPositions) { if (pathPlanner == null) { System.out.println("警告: 路径规划器未设置，无法查找等待位置"); return null; } Map<String, Map<String, Integer>> pathMappingToUse = pathMapping; if (pathMappingToUse == null && pathPlanner != null) { pathMappingToUse = pathPlanner.getPathMapping(); } if (pathMappingToUse == null) { System.out.println("警告: 路径映射表未设置，无法查找等待位置"); return null; } CTUPhysicalConfig config = agvStatus != null && agvStatus.getPhysicalConfig() != null ? agvStatus.getPhysicalConfig() : new CTUPhysicalConfig(); // 安全距离要求 int minTargetDistanceSteps = 5; int[] conflictCoord = JsonUtils.getCoordinate(conflictPosition, pathMappingToUse); if (conflictCoord == null) { System.out.println("警告: 无法获取冲突位置的坐标"); return null; } // 使用BFS搜索满足距离要求的位置 Queue<String> queue = new LinkedList<>(); Map<String, Integer> visited = new HashMap<>(); Set<String> blockedPositions = new HashSet<>(); blockedPositions.add(conflictPosition); if (overlappingPositions != null) { blockedPositions.addAll(overlappingPositions); } int[] currentCoord = JsonUtils.getCoordinate(currentPosition, pathMappingToUse); if (currentCoord == null) { System.out.println("警告: 无法获取当前位置的坐标"); return null; } queue.offer(currentPosition); visited.put(currentPosition, 0); final int MAX_SEARCH_DEPTH = 20; List<CandidatePosition> candidates = new ArrayList<>(); while (!queue.isEmpty()) { String pos = queue.poll(); int depth = visited.get(pos); if (depth >= MAX_SEARCH_DEPTH) { break; } int[] posCoord = JsonUtils.getCoordinate(pos, pathMappingToUse); if (posCoord == null) { continue; } int distanceToConflict = Math.abs(posCoord[0] - conflictCoord[0]) + Math.abs(posCoord[1] - conflictCoord[1]); int distanceToCurrent = Math.abs(posCoord[0] - currentCoord[0]) + Math.abs(posCoord[1] - currentCoord[1]); if (distanceToConflict >= minTargetDistanceSteps) { if (!blockedPositions.contains(pos)) { candidates.add(new CandidatePosition(pos, distanceToConflict, distanceToCurrent)); } } List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos); if (neighbors != null) { for (Map<String, String> neighbor : neighbors) { String neighborPos = neighbor.get("code"); if (neighborPos == null || visited.containsKey(neighborPos)) { continue; } visited.put(neighborPos, depth + 1); queue.offer(neighborPos); } } } if (!candidates.isEmpty()) { candidates.sort((c1, c2) -> { int compare = Integer.compare(c1.distanceToCurrent, c2.distanceToCurrent); if (compare != 0) { return compare; } // 如果距离当前位置相同，选择距离冲突点更近的 return Integer.compare(c1.distanceToConflict, c2.distanceToConflict); }); CandidatePosition best = candidates.get(0); System.out.println("找到安全等待位置: " + best.position + "，距离冲突点 " + best.distanceToConflict + " 步" + "，距离当前位置 " + best.distanceToCurrent + " 步"); return best.position; } System.out.println("警告: 无法找到满足距离要求的安全等待位置"); return null; } /** * 在原始路径上找一个距离冲突点足够远的位置 */ private String findWaitPositionOnOriginalPath(PlannedPath originalPath, String conflictPosition, AGVStatus agvStatus, Set<String> overlappingPositions) { if (originalPath == null || originalPath.getCodeList() == null || originalPath.getCodeList().isEmpty()) { return null; } Map<String, Map<String, Integer>> pathMappingToUse = pathMapping; if (pathMappingToUse == null && pathPlanner != null) { pathMappingToUse = pathPlanner.getPathMapping(); } if (pathMappingToUse == null) { return null; } int[] conflictCoord = JsonUtils.getCoordinate(conflictPosition, pathMappingToUse); if (conflictCoord == null) { return null; } // 安全距离要求：距离冲突点至少5步 int minTargetDistanceSteps = 5; List<PathCode> codeList = originalPath.getCodeList(); List<CandidatePosition> candidates = new ArrayList<>(); int conflictIndex = -1; for (int i = 0; i < codeList.size(); i++) { if (conflictPosition.equals(codeList.get(i).getCode())) { conflictIndex = i; break; } } if (conflictIndex < 0) { System.out.println("[调试] 原始路径中未找到冲突位置 " + conflictPosition); return null; } int checkedCount = 0; int blockedCount = 0; int tooCloseCount = 0; for (int i = 0; i < conflictIndex; i++) { String pos = codeList.get(i).getCode(); checkedCount++; if (overlappingPositions != null && overlappingPositions.contains(pos)) { blockedCount++; continue; } int[] posCoord = JsonUtils.getCoordinate(pos, pathMappingToUse); if (posCoord == null) { continue; } int distanceToConflict = Math.abs(posCoord[0] - conflictCoord[0]) + Math.abs(posCoord[1] - conflictCoord[1]); int distanceToStart = i; if (distanceToConflict >= minTargetDistanceSteps) { candidates.add(new CandidatePosition(pos, distanceToConflict, distanceToStart)); } else { tooCloseCount++; } } System.out.println("[调试] 原始路径检查: 冲突位置=" + conflictPosition + ", 索引=" + conflictIndex + ", 检查了" + checkedCount + "个点, " + "重合位置=" + blockedCount + ", " + "距离太近=" + tooCloseCount + ", " + "候选位置=" + candidates.size()); if (!candidates.isEmpty()) { candidates.sort((c1, c2) -> { int compare = Integer.compare(c1.distanceToConflict, c2.distanceToConflict); if (compare != 0) { return compare; } return Integer.compare(c2.distanceToCurrent, c1.distanceToCurrent); }); CandidatePosition best = candidates.get(0); System.out.println("在原始路径上找到等待位置: " + best.position + "，距离冲突点 " + best.distanceToConflict + " 步" + "，在路径中的位置: 第" + (best.distanceToCurrent + 1) + "个点"); return best.position; } System.out.println("[调试] 原始路径上没有找到合适的等待位置（距离冲突点至少5步）"); return null; } /** * 候选位置内部类 */ private static class CandidatePosition { final String position; final int distanceToConflict; final int distanceToCurrent; CandidatePosition(String position, int distanceToConflict, int distanceToCurrent) { this.position = position; this.distanceToConflict = distanceToConflict; this.distanceToCurrent = distanceToCurrent; } } /** * 规划到等待位置的路径 */ private PlannedPath planPathToWaitPosition(String agvId, String currentPosition, String waitPosition, AGVStatus agvStatus, String originalSegId, ExecutingTask task, long unifiedTimestamp) { if (pathPlanner == null) { return null; } CTUPhysicalConfig config = agvStatus != null && agvStatus.getPhysicalConfig() != null ? agvStatus.getPhysicalConfig() : new CTUPhysicalConfig(); try { PlannedPath path = pathPlanner.planPath(currentPosition, waitPosition); if (path != null) { path.setAgvId(agvId); String segId; if (originalSegId != null && !originalSegId.isEmpty()) { segId = originalSegId; } else if (task != null && task.getTaskId() != null) { segId = task.getTaskId() + "_" + agvId + "_" + (task.getTaskType() != null ? task.getTaskType() : "waiting"); } else { segId = "UNKNOWN_" + agvId + "_waiting"; System.out.println("警告: AGV " + agvId + " 无法获取任务信息，使用默认segId: " + segId); } path.setSegId(segId); // 使用统一时间戳（转换为毫秒） long startTimeMs = unifiedTimestamp * 1000; if (timeCalculator != null && path.getCodeList() != null) { timeCalculator.calculatePathTiming(path, startTimeMs, config, 0.0); } if (!path.getCodeList().isEmpty()) { PathCode lastCode = path.getCodeList().get(path.getCodeList().size() - 1); lastCode.setTargetPoint(true); lastCode.setActionType(null); lastCode.setPosType(null); } return path; } } catch (Exception e) { System.out.println("规划到等待位置的路径失败: " + e.getMessage()); } return null; } /** * 截断路径到等待位置 * * @param path 路径 * @param waitPosition 等待位置 */ private void truncatePathToWaitPosition(PlannedPath path, String waitPosition) { if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) { return; } List<PathCode> codeList = path.getCodeList(); List<PathCode> truncatedList = new ArrayList<>(); int waitIndex = -1; for (int i = 0; i < codeList.size(); i++) { if (codeList.get(i).getCode().equals(waitPosition)) { waitIndex = i; break; } } if (waitIndex >= 0) { truncatedList.addAll(codeList.subList(0, waitIndex + 1)); if (!truncatedList.isEmpty()) { PathCode lastCode = truncatedList.get(truncatedList.size() - 1); lastCode.setTargetPoint(true); lastCode.setActionType(null); lastCode.setPosType(null); } path.setCodeList(truncatedList); System.out.println("路径已截断到等待位置 " + waitPosition); } } /** * 截断路径到当前位置（ */ private void truncatePathToCurrentPosition(PlannedPath path, String currentPosition) { if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) { return; } List<PathCode> codeList = path.getCodeList(); List<PathCode> truncatedList = new ArrayList<>(); int currentIndex = -1; for (int i = 0; i < codeList.size(); i++) { if (codeList.get(i).getCode().equals(currentPosition)) { currentIndex = i; break; } } if (currentIndex >= 0) { truncatedList.addAll(codeList.subList(0, currentIndex + 1)); if (!truncatedList.isEmpty()) { PathCode lastCode = truncatedList.get(truncatedList.size() - 1); lastCode.setTargetPoint(true); lastCode.setActionType(null); lastCode.setPosType(null); } path.setCodeList(truncatedList); System.out.println("路径已截断到当前位置 " + currentPosition); } } /** * 评估AGV优先级 * * @param agvId AGV编号 * @param path 路径 @@ -257,7 +899,7 @@ } /** * 为路径添加延迟（增强版，正确重计算时间窗） * 为路径添加延迟 * * @param path 路径 * @param timeStep 延迟开始的时间步 algo-zkd/src/main/java/com/algo/service/ExecutingTaskExtractor.java
@@ -106,17 +106,38 @@ */ private List<ExecutingTask> extractAgvTasks(AGVStatus agvStatus) { List<ExecutingTask> tasks = new ArrayList<>(); // 检查AGV是否可用 if (!agvStatus.isAvailable()) { return tasks; } System.out.println(" [调试] 正在提取 AGV " + agvStatus.getAgvId() + " 的任务"); System.out.println(" [调试] isAvailable: " + agvStatus.isAvailable()); // 分析背篓状态 List<BackpackData> backpack = agvStatus.getBackpack(); if (backpack == null || backpack.isEmpty()) { System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 背篓为空，跳过"); return tasks; } // 检查是否有已装载的任务（即使AGV不可用，如果有已装载任务也需要规划路径） boolean hasLoadedTasks = false; for (BackpackData bp : backpack) { if (bp.isLoaded() && bp.getTaskId() != null && !bp.getTaskId().trim().isEmpty()) { hasLoadedTasks = true; break; } } // 如果AGV不可用且没有已装载任务，跳过 if (!agvStatus.isAvailable() && !hasLoadedTasks) { System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 不可用且无已装载任务，跳过"); return tasks; } // 如果有已装载任务但AGV不可用，记录日志但继续处理 if (!agvStatus.isAvailable() && hasLoadedTasks) { System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 不可用但有已装载任务，继续提取任务"); } System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 背篓数量: " + backpack.size()); // 分别收集已装载和未装载的任务 List<BackpackData> loadedTasks = new ArrayList<>(); @@ -124,21 +145,39 @@ for (BackpackData bp : backpack) { if (bp.getTaskId() != null && !bp.getTaskId().trim().isEmpty()) { System.out.println(" [调试] 发现背篓任务: taskId=" + bp.getTaskId() + ", loaded=" + bp.isLoaded() + ", index=" + bp.getIndex()); TaskData taskData = getTaskData(bp.getTaskId()); if (taskData != null) { System.out.println(" [调试] 成功获取TaskData: " + taskData); if (bp.isLoaded()) { loadedTasks.add(bp); } else { unloadedTasks.add(bp); } } else { System.out.println(" [调试] 警告：taskId=" + bp.getTaskId() + " 在taskDataMap中未找到！"); System.out.println(" [调试] taskDataMap包含的key: " + taskDataMap.keySet()); } } } System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " - 已装载任务: " + loadedTasks.size() + ", 未装载任务: " + unloadedTasks.size()); // 确定下一步最优行动 ExecutingTask nextTask = determineNextBestAction(agvStatus, loadedTasks, unloadedTasks); if (nextTask != null) { System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 生成任务: " + nextTask.getTaskType() + " " + nextTask.getCurrentPosition() + " -> " + nextTask.getTargetPosition()); tasks.add(nextTask); } else { System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " determineNextBestAction返回null"); } return tasks; @@ -157,28 +196,47 @@ List<BackpackData> loadedTasks, List<BackpackData> unloadedTasks) { String currentPosition = agvStatus.getPosition(); System.out.println(" [调试] determineNextBestAction - AGV " + agvStatus.getAgvId() + " 当前位置: " + currentPosition); List<TaskOption> taskOptions = new ArrayList<>(); // 1. 收集所有可选的送货任务 System.out.println(" [调试] 处理已装载任务数量: " + loadedTasks.size()); for (BackpackData task : loadedTasks) { TaskData taskData = getTaskData(task.getTaskId()); if (taskData != null && taskData.getEnd() != null) { System.out.println(" [调试] 计算送货距离: " + currentPosition + " -> " + taskData.getEnd()); double distance = calculateDistance(currentPosition, taskData.getEnd()); System.out.println(" [调试] 距离计算结果: " + distance); if (distance >= 0) { double cost = calculateTaskCost(distance, "delivery", taskData.getPriority(), agvStatus); taskOptions.add(new TaskOption(task, true, distance, cost, "delivery")); System.out.println(" [调试] 添加送货选项，成本: " + cost); } else { System.out.println(" [调试] 警告：距离为负，跳过此送货任务"); } } else { System.out.println(" [调试] 警告：TaskData或end为null，跳过"); } } // 2. 收集所有可选的取货任务 System.out.println(" [调试] 处理未装载任务数量: " + unloadedTasks.size()); for (BackpackData task : unloadedTasks) { TaskData taskData = getTaskData(task.getTaskId()); if (taskData != null && taskData.getStart() != null) { System.out.println(" [调试] 计算取货距离: " + currentPosition + " -> " + taskData.getStart()); double distance = calculateDistance(currentPosition, taskData.getStart()); System.out.println(" [调试] 距离计算结果: " + distance); if (distance >= 0) { double cost = calculateTaskCost(distance, "pickup", taskData.getPriority(), agvStatus); taskOptions.add(new TaskOption(task, false, distance, cost, "pickup")); System.out.println(" [调试] 添加取货选项，成本: " + cost); } else { System.out.println(" [调试] 警告：距离为负，跳过此取货任务"); } } } @@ -196,7 +254,9 @@ } // 4. 选择成本最低的任务 System.out.println(" [调试] 总任务选项数量: " + taskOptions.size()); if (taskOptions.isEmpty()) { System.out.println(" [调试] 警告：没有可选任务，返回null"); return null; } @@ -409,17 +469,33 @@ */ private double calculateDistance(String pos1, String pos2) { if (pos1 == null || pos2 == null) { System.out.println(" [调试] 警告：位置为null: pos1=" + pos1 + ", pos2=" + pos2); return -1; } // 如果位置相同，返回0 if (pos1.equals(pos2)) { System.out.println(" [调试] 位置相同，返回距离0"); return 0; } int[] coord1 = JsonUtils.getCoordinate(pos1, pathMapping); int[] coord2 = JsonUtils.getCoordinate(pos2, pathMapping); if (coord1 == null || coord2 == null) { System.out.println(" [调试] 警告：坐标查找失败!"); System.out.println(" pos1=" + pos1 + " -> coord1=" + (coord1 != null ? "[" + coord1[0] + "," + coord1[1] + "]" : "null")); System.out.println(" pos2=" + pos2 + " -> coord2=" + (coord2 != null ? "[" + coord2[0] + "," + coord2[1] + "]" : "null")); System.out.println(" pathMapping包含pos1? " + pathMapping.containsKey(pos1)); System.out.println(" pathMapping包含pos2? " + pathMapping.containsKey(pos2)); return -1; } return JsonUtils.calculateManhattanDistance(coord1, coord2); double distance = JsonUtils.calculateManhattanDistance(coord1, coord2); System.out.println(" [调试] 曼哈顿距离: " + distance + " (" + pos1 + " -> " + pos2 + ")"); return distance; } /** algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -84,8 +84,10 @@ 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()); } /** @@ -127,7 +129,6 @@ // 3. 分类处理CTU：有剩余路径的和需要新路径的 List<AGVStatus> ctuWithRemainingPaths = new ArrayList<>(); List<AGVStatus> ctuNeedingNewPaths = new ArrayList<>(); List<AGVStatus> unhandledAgvs = new ArrayList<>(); // 未处理的AGV for (AGVStatus agv : agvStatusList) { // 故障车已在时空占用表中处理，标记为已处理但不规划路径 @@ -137,32 +138,27 @@ } if (agv.hasRemainingPath()) { // 有剩余路径的AGV，先处理剩余路径 ctuWithRemainingPaths.add(agv); } else if (agv.canAcceptNewTask()) { // 可以接受新任务的AGV（有任务或可以接受新任务） ctuNeedingNewPaths.add(agv); } else if (includeIdleAgv && isAgvIdle(agv)) { // 完全空闲的AGV（无任务，无剩余路径） ctuNeedingNewPaths.add(agv); } else { // 其他情况：不可用、有剩余路径但>1、有任务但不可用等 unhandledAgvs.add(agv); // 只要不是故障车且没有剩余路径，参与路径规划 ctuNeedingNewPaths.add(agv); } } System.out.println("CTU分类完成: 有剩余路径=" + ctuWithRemainingPaths.size() + ", 需要新路径=" + ctuNeedingNewPaths.size() + ", 未处理=" + unhandledAgvs.size()); ", 需要新路径=" + ctuNeedingNewPaths.size()); // 记录未处理的AGV信息（debug） if (!unhandledAgvs.isEmpty()) { for (AGVStatus agv : unhandledAgvs) { // 记录需要新路径的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() + ", canAcceptNewTask=" + agv.canAcceptNewTask() + ", isIdle=" + isAgvIdle(agv) + ", isAvailable=" + agv.isAvailable()); ", isAvailable=" + agv.isAvailable() + ", position=" + agv.getPosition()); } } @@ -308,7 +304,7 @@ if (!conflicts.isEmpty()) { System.out.println("发现 " + conflicts.size() + " 个碰撞，开始解决"); plannedPaths = collisionResolver.resolveConflicts(plannedPaths, conflicts, executingTasks); plannedPaths = collisionResolver.resolveConflicts(plannedPaths, conflicts, executingTasks, agvStatusList, unifiedTimestamp); // 重新检测冲突 List<Conflict> remainingConflicts = collisionDetector.detectConflicts(plannedPaths);
