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 | 117 ++++++++++++++++++++++++++++++++++++++--------------------
1 files changed, 77 insertions(+), 40 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 61d625d..1d93b85 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -103,41 +103,70 @@
this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);
long startTime = System.currentTimeMillis();
- long unifiedTimestamp = startTime / 1000; // 缁熶竴鏃堕棿鎴筹紙绉掞級
+ long unifiedTimestamp = startTime / 1000;
System.out.println("寮�濮嬩负 " + agvStatusList.size() + " 涓狢TU瑙勫垝");
- System.out.println("缁熶竴鏃堕棿鍩哄噯: " + unifiedTimestamp + "绉� (" + new Date(startTime) + ")");
+ 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("妫�娴嬪埌 " + 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) {
@@ -157,13 +186,13 @@
}
}
- // 3.5. 妫�鏌ョ┖闂睞GV鏄惁闇�瑕佽琛�
+ // 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);
+ PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
if (yieldPath != null) {
plannedPaths.add(yieldPath);
plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -175,12 +204,12 @@
System.out.println(" 鏃犻渶璁╄鐨凙GV");
}
- // 4. 涓洪渶瑕佹柊璺緞鐨凜TU鎻愬彇鎵ц涓换鍔�
+ // 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();
@@ -208,7 +237,7 @@
// 瑙勫垝鏃剁┖瀹夊叏鐨勮矾寰�
PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
+ currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
);
if (plannedPath != null) {
@@ -216,7 +245,6 @@
plannedPath.setAgvId(agvId);
plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));
- // 澧炲己璺緞浠ｇ爜淇℃伅
enhancePathCodes(plannedPath, task);
plannedPaths.add(plannedPath);
@@ -233,11 +261,11 @@
}
}
- // 6. 澶勭悊绌洪棽CTU
+ // 7. 澶勭悊绌洪棽CTU
if (includeIdleAgv) {
System.out.println("姝ラ5: 澶勭悊绌洪棽CTU");
- // 6.1. 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤宸茶鍒掕矾寰勶紝濡傛灉鍗犵敤鍒欑敓鎴愰伩璁╄矾寰�
+ // 7.1. 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤宸茶鍒掕矾寰勶紝濡傛灉鍗犵敤鍒欑敓鎴愰伩璁╄矾寰�
List<AGVStatus> yieldingIdleAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
List<AGVStatus> yieldingAgvsToHandle = new ArrayList<>();
@@ -250,7 +278,7 @@
if (!yieldingAgvsToHandle.isEmpty()) {
System.out.println( yieldingAgvsToHandle.size() + " 涓渶瑕侀伩璁╃殑绌洪棽AGV");
for (AGVStatus yieldAgv : yieldingAgvsToHandle) {
- PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
+ PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
if (yieldPath != null) {
plannedPaths.add(yieldPath);
plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -260,18 +288,18 @@
System.out.println(" 鏃犻渶閬胯鐨勭┖闂睞GV");
}
- // 6.2. 鐒跺悗涓哄墿浣欑殑绌洪棽AGV瑙勫垝寰呮満/鍏呯數璺緞
+ // 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());
}
- // 6.5. 鏈�缁堥伩璁╂鏌�
- performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints);
+ // 7.5. 鏈�缁堥伩璁╂鏌�
+ performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
- // 7. 鏈�缁堢鎾炴娴嬪拰瑙ｅ喅锛堥拡瀵瑰悓鏃剁敓鎴愮殑鏂拌矾寰勶級
+ // 8. 鏈�缁堢鎾炴娴嬪拰瑙ｅ喅
System.out.println("姝ラ6: 鏈�缁堢鎾炴娴�");
List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);
@@ -415,27 +443,30 @@
* @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) {
+ AGVStatus agvStatus,
+ long unifiedTimestamp) {
// 甯︽椂绌哄崰鐢ㄨ〃鐨勮矾寰勮鍒�
+ long startTimeMs = unifiedTimestamp * 1000;
PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
startPos,
endPos,
constraints,
occupancyMap,
- agvStatus.getPhysicalConfig()
+ agvStatus.getPhysicalConfig(),
+ startTimeMs
);
if (spacetimePath == null) {
return null;
}
- long startTime = System.currentTimeMillis();
- timeCalculator.calculatePathTiming(spacetimePath, startTime, agvStatus.getPhysicalConfig(), 0.0);
+ timeCalculator.calculatePathTiming(spacetimePath, startTimeMs, agvStatus.getPhysicalConfig(), 0.0);
return spacetimePath;
}
@@ -526,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) {
@@ -549,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) {
@@ -709,7 +742,8 @@
List<PlannedPath> plannedPaths,
Map<String, String> plannedAgvIds,
Map<String, String> spaceTimeOccupancyMap,
- List<double[]> constraints) {
+ List<double[]> constraints,
+ long unifiedTimestamp) {
final int MAX_ITERATIONS = 5; // 鏈�澶ц凯浠ｆ鏁�
int iteration = 0;
int totalYieldingCount = 0;
@@ -738,7 +772,7 @@
continue;
}
- PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
+ PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
if (yieldPath != null) {
plannedPaths.add(yieldPath);
plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -827,7 +861,8 @@
* @return 璁╄璺緞
*/
private PlannedPath planYieldPath(AGVStatus yieldAgv, List<PlannedPath> existingPaths,
- Map<String, String> occupancyMap, List<double[]> constraints) {
+ Map<String, String> occupancyMap, List<double[]> constraints,
+ long unifiedTimestamp) {
String currentPos = yieldAgv.getPosition();
String agvId = yieldAgv.getAgvId();
@@ -853,7 +888,7 @@
// 3. 瑙勫垝閬胯璺緞
PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, occupancyMap, yieldAgv
+ currentPos, targetPos, constraints, occupancyMap, yieldAgv, unifiedTimestamp
);
if (yieldPath != null) {
@@ -1114,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<>());
@@ -1135,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);
}
@@ -1165,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();
@@ -1181,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