From 7dd762215b373851ed313b46bad08cc973816665 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期三, 12 十一月 2025 12:26:23 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/CollisionResolver.java | 734 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 724 insertions(+), 10 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/service/CollisionResolver.java b/algo-zkd/src/main/java/com/algo/service/CollisionResolver.java
index cdf70d7..537e513 100644
--- a/algo-zkd/src/main/java/com/algo/service/CollisionResolver.java
+++ b/algo-zkd/src/main/java/com/algo/service/CollisionResolver.java
@@ -1,9 +1,13 @@
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.*;
@@ -17,6 +21,21 @@
* 纰版挒妫�娴嬪櫒
*/
private final CollisionDetector collisionDetector;
+
+ /**
+ * 璺緞鏃堕棿璁＄畻鍣�
+ */
+ private PathTimeCalculator timeCalculator;
+
+ /**
+ * 璺緞瑙勫垝鍣�
+ */
+ private PathPlanner pathPlanner;
+
+ /**
+ * 璺緞鏄犲皠琛紙鐢ㄤ簬鍧愭爣杞崲锛�
+ */
+ private Map<String, Map<String, Integer>> pathMapping;
/**
* 鏋勯�犲嚱鏁�
@@ -26,6 +45,27 @@
public CollisionResolver(CollisionDetector collisionDetector) {
this.collisionDetector = collisionDetector;
}
+
+ /**
+ * 璁剧疆鏃堕棿璁＄畻鍣�
+ */
+ 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;
+ }
/**
* 瑙ｅ喅璺緞鍐茬獊
@@ -33,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;
}
@@ -49,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);
@@ -56,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());
@@ -67,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) {
@@ -89,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);
@@ -163,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();
@@ -186,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;
+ }
+
+ /**
+ * 瑙ｅ喅鏃堕棿闂撮殧涓嶈冻鍐茬獊
+ * 绛栫暐锛氭湁鍓╀綑璺緞鐨凙GV缁х画鎵ц锛屾柊璺緞鐨凙GV绉诲姩鍒板畨鍏ㄧ瓑寰呬綅缃�
+ */
+ 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鏄柊璺緞锛孉GV2鏈夊墿浣欒矾寰� -> 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浼樺厛绾ч珮锛孉GV2绛夊緟
+ waitingAgv = secondAgv;
+ waitingPath = path2;
+ waitingAgvStatus = agv2Status;
+ System.out.println("鍐茬獊瑙ｅ喅: 涓や釜閮芥湁鍓╀綑璺緞锛孉GV " + firstAgv + " 浼樺厛绾ч珮缁х画鎵ц锛孉GV " + secondAgv + " 绛夊緟");
+ } else {
+ // AGV2浼樺厛绾ч珮锛孉GV1绛夊緟
+ waitingAgv = firstAgv;
+ waitingPath = path1;
+ waitingAgvStatus = agv1Status;
+ System.out.println("鍐茬獊瑙ｅ喅: 涓や釜閮芥湁鍓╀綑璺緞锛孉GV " + secondAgv + " 浼樺厛绾ч珮缁х画鎵ц锛孉GV " + 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 + " 浼樺厛绾ч珮缁х画鎵ц锛孉GV " + secondAgv + " 绛夊緟");
+ } else {
+ waitingAgv = firstAgv;
+ waitingPath = path1;
+ waitingAgvStatus = agv1Status;
+ System.out.println("鍐茬獊瑙ｅ喅: 涓や釜閮芥槸鏂拌矾寰勶紝AGV " + secondAgv + " 浼樺厛绾ч珮缁х画鎵ц锛孉GV " + 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;
+ }
+
+ // 妫�鏌ョ瓑寰匒GV鐨勮矾寰勬槸鍚︾粡杩囧啿绐佷綅缃�
+ 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("璀﹀憡: 鏃犳硶鎵惧埌瀹夊叏绛夊緟浣嶇疆锛孉GV " + 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("璀﹀憡: 鏃犳硶瑙勫垝鍒扮瓑寰呬綅缃殑璺緞锛孉GV " + 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 + " 鏃犳硶鑾峰彇浠诲姟淇℃伅锛屼娇鐢ㄩ粯璁egId: " + 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 璺緞
@@ -263,6 +919,9 @@
// 鑾峰彇寤惰繜浣嶇疆鐨勮矾寰勪唬鐮�
PathCode delayCode = codeList.get(timeStep);
+
+ // 璁＄畻寤惰繜鏃堕棿锛堟瘡涓欢杩熸楠�1绉掞級
+ long delayDuration = delaySteps * 1000L;
// 鍒涘缓寤惰繜姝ラ
List<PathCode> delaySteps_list = new ArrayList<>();
@@ -273,14 +932,69 @@
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(); // 浣跨敤榛樿鏋勯�犲嚱鏁�
}
/**
--
Gitblit v1.9.1