From 12f30a09bce1c61b2ae90129124cdc467a59b074 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期三, 05 十一月 2025 15:03:17 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 540 +++++++++++++++++++++++++++++++++++++++++++++++++++++++----
1 files changed, 496 insertions(+), 44 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
index 026b64b..1d93b85 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -4,7 +4,6 @@
import com.algo.model.*;
import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;
-import lombok.Data;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;
@@ -104,33 +103,70 @@
this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);
long startTime = System.currentTimeMillis();
+ long unifiedTimestamp = startTime / 1000;
System.out.println("寮�濮嬩负 " + agvStatusList.size() + " 涓狢TU瑙勫垝");
+ 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);
+ Map<String, String> spaceTimeOccupancyMap = remainingPathProcessor.buildSpaceTimeOccupancyMap(agvStatusList, unifiedTimestamp);
System.out.println("鏃剁┖鍗犵敤琛ㄦ瀯寤哄畬鎴愶紝鍖呭惈 " + spaceTimeOccupancyMap.size() + " 涓崰鐢ㄧ偣");
- // 2. 鍒嗙被澶勭悊CTU锛氭湁鍓╀綑璺緞鐨勫拰闇�瑕佹柊璺緞鐨�
+ // 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<>();
+ List<AGVStatus> unhandledAgvs = new ArrayList<>(); // 鏈鐞嗙殑AGV
for (AGVStatus agv : agvStatusList) {
+ // 鏁呴殰杞﹀凡鍦ㄦ椂绌哄崰鐢ㄨ〃涓鐞嗭紝鏍囪涓哄凡澶勭悊浣嗕笉瑙勫垝璺緞
+ if (agv.getError() == 1) {
+ plannedAgvIds.put(agv.getAgvId(), "FAULTY");
+ continue;
+ }
+
if (agv.hasRemainingPath()) {
ctuWithRemainingPaths.add(agv);
- } else if (agv.canAcceptNewTask() || (includeIdleAgv && isAgvIdle(agv))) {
+ } else if (agv.canAcceptNewTask()) {
+ // 鍙互鎺ュ彈鏂颁换鍔＄殑AGV锛堟湁浠诲姟鎴栧彲浠ユ帴鍙楁柊浠诲姟锛�
ctuNeedingNewPaths.add(agv);
+ } else if (includeIdleAgv && isAgvIdle(agv)) {
+ // 瀹屽叏绌洪棽鐨凙GV锛堟棤浠诲姟锛屾棤鍓╀綑璺緞锛�
+ ctuNeedingNewPaths.add(agv);
+ } else {
+ // 鍏朵粬鎯呭喌锛氫笉鍙敤銆佹湁鍓╀綑璺緞浣�>1銆佹湁浠诲姟浣嗕笉鍙敤绛�
+ unhandledAgvs.add(agv);
}
}
System.out.println("CTU鍒嗙被瀹屾垚: 鏈夊墿浣欒矾寰�=" + ctuWithRemainingPaths.size() +
- ", 闇�瑕佹柊璺緞=" + ctuNeedingNewPaths.size());
+ ", 闇�瑕佹柊璺緞=" + ctuNeedingNewPaths.size() +
+ ", 鏈鐞�=" + unhandledAgvs.size());
+
+ // 璁板綍鏈鐞嗙殑AGV淇℃伅锛坉ebug锛�
+ if (!unhandledAgvs.isEmpty()) {
+ for (AGVStatus agv : unhandledAgvs) {
+ System.out.println(" - AGV " + agv.getAgvId() +
+ ": error=" + agv.getError() +
+ ", hasRemainingPath=" + agv.hasRemainingPath() +
+ ", canAcceptNewTask=" + agv.canAcceptNewTask() +
+ ", isIdle=" + isAgvIdle(agv) +
+ ", isAvailable=" + agv.isAvailable());
+ }
+ }
- // 3. 澶勭悊鏈夊墿浣欒矾寰勭殑CTU
- List<PlannedPath> plannedPaths = new ArrayList<>();
- List<ExecutingTask> executingTasks = new ArrayList<>();
- Map<String, String> plannedAgvIds = new HashMap<>();
+ // 4. 澶勭悊鏈夊墿浣欒矾寰勭殑CTU
System.out.println("姝ラ2: 澶勭悊鏈夊墿浣欒矾寰勭殑CTU");
for (AGVStatus agv : ctuWithRemainingPaths) {
@@ -150,12 +186,30 @@
}
}
- // 4. 涓洪渶瑕佹柊璺緞鐨凜TU鎻愬彇鎵ц涓换鍔�
+ // 4.5. 妫�鏌ョ┖闂睞GV鏄惁闇�瑕佽琛�
+ System.out.println("妫�鏌ョ┖闂睞GV鏄惁闇�瑕佽琛�");
+ 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(" 鏃犻渶璁╄鐨凙GV");
+ }
+
+ // 5. 涓洪渶瑕佹柊璺緞鐨凜TU鎻愬彇鎵ц涓换鍔�
System.out.println("姝ラ3: 鎻愬彇闇�瑕佹柊璺緞鐨凜TU浠诲姟");
List<ExecutingTask> newTasks = taskExtractor.extractExecutingTasks(ctuNeedingNewPaths);
System.out.println("鎻愬彇鍒� " + newTasks.size() + " 涓柊浠诲姟");
- // 5. 涓烘柊浠诲姟瑙勫垝璺緞锛堣�冭檻鏃剁┖绾︽潫锛�
+ // 6. 涓烘柊浠诲姟瑙勫垝璺緞锛堣�冭檻鏃剁┖绾︽潫锛�
System.out.println("姝ラ4: 涓烘柊浠诲姟瑙勫垝璺緞");
for (ExecutingTask task : newTasks) {
String agvId = task.getAgvId();
@@ -183,7 +237,7 @@
// 瑙勫垝鏃剁┖瀹夊叏鐨勮矾寰�
PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
+ currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
);
if (plannedPath != null) {
@@ -191,7 +245,6 @@
plannedPath.setAgvId(agvId);
plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));
- // 澧炲己璺緞浠ｇ爜淇℃伅
enhancePathCodes(plannedPath, task);
plannedPaths.add(plannedPath);
@@ -208,17 +261,45 @@
}
}
- // 6. 澶勭悊绌洪棽CTU
+ // 7. 澶勭悊绌洪棽CTU
if (includeIdleAgv) {
System.out.println("姝ラ5: 澶勭悊绌洪棽CTU");
+
+ // 7.1. 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤宸茶鍒掕矾寰勶紝濡傛灉鍗犵敤鍒欑敓鎴愰伩璁╄矾寰�
+ 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(" 鏃犻渶閬胯鐨勭┖闂睞GV");
+ }
+
+ // 7.2. 鐒跺悗涓哄墿浣欑殑绌洪棽AGV瑙勫垝寰呮満/鍏呯數璺緞
List<PlannedPath> idlePaths = planIdleAgvPathsWithConstraints(
- ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap
+ ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap, unifiedTimestamp
);
plannedPaths.addAll(idlePaths);
- System.out.println("绌洪棽CTU璺緞瑙勫垝瀹屾垚锛屾暟閲�: " + idlePaths.size());
+ System.out.println(" 绌洪棽CTU璺緞瑙勫垝锛屾暟閲�: " + idlePaths.size());
}
- // 7. 鏈�缁堢鎾炴娴嬪拰瑙ｅ喅锛堥拡瀵瑰悓鏃剁敓鎴愮殑鏂拌矾寰勶級
+ // 7.5. 鏈�缁堥伩璁╂鏌�
+ performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
+
+ // 8. 鏈�缁堢鎾炴娴嬪拰瑙ｅ喅
System.out.println("姝ラ6: 鏈�缁堢鎾炴娴�");
List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);
@@ -270,17 +351,51 @@
PlannedPath remainingPath = agv.getRemainingPath();
List<PathCode> remainingCodes = new ArrayList<>();
- // 浠庡綋鍓嶄綅缃紑濮嬶紝鑾峰彇鍓╀綑璺緞
+ // 鑾峰彇鍓╀綑璺緞
List<PathCode> originalCodes = remainingPath.getCodeList();
for (int i = agv.getCurrentPathIndex(); i < originalCodes.size(); i++) {
- remainingCodes.add(originalCodes.get(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);
- processedPath.setSegId(agv.getAgvId() + "_REMAINING_" + System.currentTimeMillis());
+
+ // 浣跨敤杈撳叆涓殑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()) {
+ // 鑾峰彇AGV鐨勪笅涓�涓矾寰勭偣鍒拌揪鏃堕棿浣滀负璧峰鏃堕棿
+ long startTime = agv.getNextPointArrivalTime();
+
+ CTUPhysicalConfig config = agv.getPhysicalConfig();
+
+ // 浼扮畻褰撳墠閫熷害锛圓GV绉诲姩涓负姝ｅ父閫熷害锛涢潤姝负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;
}
@@ -328,27 +443,32 @@
* @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) {
- // 灏濊瘯鍩烘湰璺緞瑙勫垝
- PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
- if (basicPath == null) {
+ 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;
}
- // 鎵惧埌瀹夊叏鐨勮捣濮嬫椂闂�
- long safeStartTime = remainingPathProcessor.findSafeStartTime(
- basicPath, occupancyMap, agvStatus.getPhysicalConfig()
- );
+ timeCalculator.calculatePathTiming(spacetimePath, startTimeMs, agvStatus.getPhysicalConfig(), 0.0);
- // 浣跨敤缁熶竴鐨勬椂闂磋绠楀櫒璁剧疆绮剧‘鐨勬椂闂翠俊鎭�
- timeCalculator.calculatePathTiming(basicPath, safeStartTime, agvStatus.getPhysicalConfig(), 0.0);
-
- return basicPath;
+ return spacetimePath;
}
/**
@@ -378,9 +498,12 @@
// 濡傛灉鏈夋柟鍚戝彉鍖栵紝澧炲姞杞悜鏃堕棿
PathCode nextCode = codeList.get(i + 1);
- if (!pathCode.getDirection().equals(nextCode.getDirection())) {
- double turnTime = config.getTurnTime(pathCode.getDirection(), nextCode.getDirection());
- currentTime += (long) (turnTime * 1000);
+ // 娣诲姞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);
+ }
}
}
}
@@ -434,12 +557,14 @@
* @param plannedAgvIds 宸茶鍒扖TU鏄犲皠
* @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) {
+ Map<String, String> occupancyMap,
+ long unifiedTimestamp) {
List<PlannedPath> idlePaths = new ArrayList<>();
for (AGVStatus agvStatus : agvStatusList) {
@@ -457,7 +582,7 @@
if (targetPos != null && !targetPos.equals(currentPos)) {
PlannedPath idlePath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, occupancyMap, agvStatus
+ currentPos, targetPos, constraints, occupancyMap, agvStatus, unifiedTimestamp
);
if (idlePath != null) {
@@ -611,9 +736,334 @@
}
/**
+ * 杩唬妫�鏌ユ墍鏈夌┖闂睞GV鏄惁鍗犵敤宸茶鍒掕矾寰�
+ */
+ 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(" 鏃犻渶閬胯鐨凙GV");
+ } 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 + " 涓伩璁〢GV锛岃凯浠� " + iteration + " 杞�");
+ }
+ }
+
+ /**
+ * 璇嗗埆闇�瑕佽琛岀殑AGV
+ * 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤浜嗗叾浠朅GV鍓╀綑璺緞涓婄殑浣嶇疆
+ *
+ * @param agvStatusList 鎵�鏈堿GV鐘舵�佸垪琛�
+ * @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());
+ }
+ }
+ }
+ }
+
+ // 妫�鏌ユ瘡涓狝GV
+ for (AGVStatus agv : agvStatusList) {
+ // 濡傛灉杩欎釜AGV宸茬粡鏈夊墿浣欒矾寰勶紝璺宠繃锛堝凡鍦ㄤ换鍔★級
+ if (agv.hasRemainingPath()) {
+ continue;
+ }
+
+ // 妫�鏌GV鏄惁鏈変綅缃俊鎭�
+ if (agv.getPosition() == null || agv.getPosition().trim().isEmpty()) {
+ continue;
+ }
+
+ // 妫�鏌GV鏄惁鏄┖闂茬姸鎬侊紙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 + " 鍗犵敤浜嗗叾浠朇TU璺緞锛岄渶瑕佽琛�");
+ yieldingAgvs.add(agv);
+ }
+ }
+
+ return yieldingAgvs;
+ }
+
+ /**
+ * 涓鸿琛孉GV瑙勫垝閬胯璺緞
+ *
+ * @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);
+ }
+ }
+
+ /**
* 璺緞瑙勫垝缁撴灉绫�
*/
- @Data
public static class PathPlanningResult {
private int totalAgvs;
private int executingTasksCount;
@@ -699,7 +1149,8 @@
private List<PlannedPath> planTasksInParallel(List<ExecutingTask> tasks,
List<double[]> constraints,
Map<String, String> spaceTimeOccupancyMap,
- List<AGVStatus> agvStatusList) {
+ List<AGVStatus> agvStatusList,
+ long unifiedTimestamp) {
List<PlannedPath> allPaths = Collections.synchronizedList(new ArrayList<>());
@@ -720,7 +1171,7 @@
for (ExecutingTask task : batch) {
Future<PlannedPath> future = executorService.submit(() -> {
- return planSingleTaskPath(task, constraints, spaceTimeOccupancyMap, agvStatusList);
+ return planSingleTaskPath(task, constraints, spaceTimeOccupancyMap, agvStatusList, unifiedTimestamp);
});
futures.add(future);
}
@@ -750,7 +1201,8 @@
private PlannedPath planSingleTaskPath(ExecutingTask task,
List<double[]> constraints,
Map<String, String> spaceTimeOccupancyMap,
- List<AGVStatus> agvStatusList) {
+ List<AGVStatus> agvStatusList,
+ long unifiedTimestamp) {
String agvId = task.getAgvId();
String currentPos = task.getCurrentPosition();
@@ -766,9 +1218,9 @@
return null;
}
- // 瑙勫垝鏃剁┖瀹夊叏鐨勮矾寰�
+ // 瑙勫垝鏃剁┖瀹夊叏鐨勮矾寰勶紙浣跨敤缁熶竴鏃堕棿鎴筹級
PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
+ currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
);
if (plannedPath != null) {
--
Gitblit v1.9.1