From 434e0ff84a7df875f223e9118f244e04f28310ca Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期二, 04 十一月 2025 14:38:56 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 32 ++++++++++-----
algo-zkd/src/main/java/com/algo/service/RemainingPathProcessor.java | 66 ++++++++++++++++++++++++++++----
algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java | 2
3 files changed, 79 insertions(+), 21 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
index cdc4431..82b7b91 100644
--- a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
+++ b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
@@ -44,7 +44,7 @@
/**
* 鏈�澶ф悳绱㈡繁搴�
*/
- private final int maxSearchDepth = 15000;
+ private final int maxSearchDepth = 50000;
/**
* 璺濈缂撳瓨
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 7ea0de9..61d625d 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -103,8 +103,16 @@
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)锛屽皢鍦ㄦ椂绌鸿〃涓崰鎹綅缃�");
+ }
// 1. 鏋勫缓鐜版湁鍓╀綑璺緞鐨勬椂绌哄崰鐢ㄨ〃
System.out.println("姝ラ1: 鏋勫缓鏃剁┖鍗犵敤琛�");
@@ -413,21 +421,23 @@
List<double[]> constraints,
Map<String, String> occupancyMap,
AGVStatus agvStatus) {
- // 灏濊瘯鍩烘湰璺緞瑙勫垝
- PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
- if (basicPath == null) {
+ // 甯︽椂绌哄崰鐢ㄨ〃鐨勮矾寰勮鍒�
+ PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
+ startPos,
+ endPos,
+ constraints,
+ occupancyMap,
+ agvStatus.getPhysicalConfig()
+ );
+
+ if (spacetimePath == null) {
return null;
}
- // 鎵惧埌瀹夊叏鐨勮捣濮嬫椂闂�
- long safeStartTime = remainingPathProcessor.findSafeStartTime(
- basicPath, occupancyMap, agvStatus.getPhysicalConfig()
- );
+ long startTime = System.currentTimeMillis();
+ timeCalculator.calculatePathTiming(spacetimePath, startTime, agvStatus.getPhysicalConfig(), 0.0);
- // 浣跨敤缁熶竴鐨勬椂闂磋绠楀櫒璁剧疆绮剧‘鐨勬椂闂翠俊鎭�
- timeCalculator.calculatePathTiming(basicPath, safeStartTime, agvStatus.getPhysicalConfig(), 0.0);
-
- return basicPath;
+ return spacetimePath;
}
/**
diff --git a/algo-zkd/src/main/java/com/algo/service/RemainingPathProcessor.java b/algo-zkd/src/main/java/com/algo/service/RemainingPathProcessor.java
index 273244d..10d8fa4 100644
--- a/algo-zkd/src/main/java/com/algo/service/RemainingPathProcessor.java
+++ b/algo-zkd/src/main/java/com/algo/service/RemainingPathProcessor.java
@@ -26,33 +26,79 @@
* 绯荤粺褰撳墠鏃堕棿鍩哄噯锛堟绉掞級
*/
private long systemBaseTime;
+ private long unifiedTimestamp;
public RemainingPathProcessor(Map<String, Map<String, Integer>> pathMapping) {
this.pathMapping = pathMapping;
this.systemBaseTime = System.currentTimeMillis();
+ this.unifiedTimestamp = this.systemBaseTime / 1000;
}
/**
* 澶勭悊鎵�鏈塁TU鐨勫墿浣欒矾寰勶紝鏋勫缓鏃剁┖鍗犵敤琛�
- * 馃敡 淇锛氬悓鏃跺鐞嗘湁鍓╀綑璺緞鐨凙GV鍜岄潤姝GV鐨勫綋鍓嶄綅缃崰鐢�
+ * 1. 浣跨敤缁熶竴鏃堕棿鎴充綔涓烘墍鏈堿GV鐨勬椂闂村熀鍑�
+ * 2. 瀵筫rror=1鐨勬晠闅滆溅锛屽叏绋嬪崰鎹椂绌鸿〃锛岀‘淇濆彲浠ョ敓鎴愮粫琛岃矾绾�
*
* @param agvStatusList CTU鐘舵�佸垪琛�
* @return 鏃剁┖鍗犵敤琛紝key涓�"x,y,timeSlot"锛寁alue涓篊TU缂栧彿
*/
public Map<String, String> buildSpaceTimeOccupancyMap(List<AGVStatus> agvStatusList) {
+ // 閲嶇疆缁熶竴鏃堕棿鎴充负褰撳墠鏃堕棿
+ this.unifiedTimestamp = System.currentTimeMillis() / 1000;
+
Map<String, String> occupancyMap = new HashMap<>();
for (AGVStatus agv : agvStatusList) {
- if (agv.hasRemainingPath()) {
- // 澶勭悊鏈夊墿浣欒矾寰勭殑AGV
+ // 澶勭悊鏁呴殰杞︼細error=1鐨勮溅杈嗗叏绋嬪崰鎹椂绌鸿〃
+ if (agv.getError() == 1) {
+ // 鏁呴殰杞﹀叏绋嬪崰鎹叾褰撳墠浣嶇疆
+ processFaultyAgvOccupancy(agv, occupancyMap);
+ } else if (agv.hasRemainingPath()) {
+ // 澶勭悊鏈夊墿浣欒矾寰勭殑姝ｅ父AGV
processRemainingPathOccupancy(agv, occupancyMap);
} else if (agv.getPosition() != null && !agv.getPosition().isEmpty()) {
- // 澶勭悊闈欐AGV鐨勫綋鍓嶄綅缃崰鐢�
+ // 澶勭悊闈欐鐨勬甯窤GV鐨勫綋鍓嶄綅缃崰鐢�
processStaticAgvOccupancy(agv, occupancyMap);
}
}
return occupancyMap;
+ }
+
+ /**
+ * 澶勭悊鏁呴殰杞︾殑鏃剁┖鍗犵敤
+ * 鏁呴殰杞�(error=1)鍦ㄦ椂绌鸿〃涓婂叏绋嬪崰鎹叾浣嶇疆
+ *
+ * @param agv 鏁呴殰CTU鐘舵��
+ * @param occupancyMap 鏃剁┖鍗犵敤琛�
+ */
+ private void processFaultyAgvOccupancy(AGVStatus agv, Map<String, String> occupancyMap) {
+ String position = agv.getPosition();
+ if (position == null || position.isEmpty()) {
+ return;
+ }
+
+ // 鑾峰彇浣嶇疆鍧愭爣
+ int[] coord = JsonUtils.getCoordinate(position, pathMapping);
+ if (coord == null) {
+ return;
+ }
+
+ CTUPhysicalConfig config = agv.getPhysicalConfig();
+
+ long occupancyDuration = 86400;
+
+ // 浠庣粺涓�鏃堕棿鎴冲紑濮嬪崰鐢�
+ for (long timeSlot = unifiedTimestamp; timeSlot < unifiedTimestamp + occupancyDuration; timeSlot++) {
+ String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
+ occupancyMap.put(spaceTimeKey, agv.getAgvId() + "_FAULT");
+
+ occupyAdjacentSpaces(coord, timeSlot, agv.getAgvId() + "_FAULT", occupancyMap, config);
+ }
+
+ System.out.println(" 鏁呴殰AGV " + agv.getAgvId() + " 鍏ㄧ▼鍗犵敤浣嶇疆 " + position +
+ " (鍧愭爣: " + coord[0] + "," + coord[1] + "), 鏃堕棿: " +
+ unifiedTimestamp + " ~ " + (unifiedTimestamp + occupancyDuration) + "绉�");
}
/**
@@ -74,12 +120,11 @@
}
CTUPhysicalConfig config = agv.getPhysicalConfig();
- long currentTime = System.currentTimeMillis() / 1000; // 杞崲涓虹
// 闈欐AGV鍗犵敤褰撳墠浣嶇疆鐨勯暱鏃堕棿娈碉紙鍋囪300绉掞級
long occupancyDuration = 300; // 300绉掔殑鍗犵敤鏃堕棿
-
- for (long timeSlot = currentTime; timeSlot < currentTime + occupancyDuration; timeSlot++) {
+
+ for (long timeSlot = unifiedTimestamp; timeSlot < unifiedTimestamp + occupancyDuration; timeSlot++) {
String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
occupancyMap.put(spaceTimeKey, agv.getAgvId());
@@ -101,8 +146,11 @@
List<PathCode> codeList = remainingPath.getCodeList();
CTUPhysicalConfig config = agv.getPhysicalConfig();
- // 浠庡綋鍓嶄綅缃紑濮嬭绠楁椂绌哄崰鐢�
- long currentTime = agv.getNextPointArrivalTime();
+ // 浠庣粺涓�鏃堕棿鎴虫垨AGV鐨勪笅涓�鐐瑰埌杈炬椂闂村紑濮嬶紝鍙栬緝澶у��
+ // 濡傛灉AGV宸茬粡鍦ㄦ墽琛岃矾寰勪笖涓嬩竴鐐瑰埌杈炬椂闂村湪鏈潵锛屽垯浣跨敤瀹為檯鏃堕棿
+ // 濡傛灉AGV鍒氬紑濮嬫垨鏃堕棿宸茶繃锛屽垯浣跨敤缁熶竴鏃堕棿鎴�
+ long agvNextArrivalTime = agv.getNextPointArrivalTime() / 1000; // 杞崲涓虹
+ long currentTime = Math.max(unifiedTimestamp, agvNextArrivalTime) * 1000; // 杞洖姣杩涜璁＄畻
int startIndex = agv.getCurrentPathIndex();
for (int i = startIndex; i < codeList.size(); i++) {
--
Gitblit v1.9.1