From 642f20dd1b068e23146b8ec96c5eac8b63c96b87 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期三, 05 十一月 2025 18:18:25 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 385 +++++++++++++++++++++++++++++++++++++++++++++++-------
1 files changed, 333 insertions(+), 52 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 e8f0fef..54b0e54 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -103,37 +103,74 @@
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) {
- PlannedPath remainingPath = processRemainingPath(agv);
+ PlannedPath remainingPath = processRemainingPath(agv, unifiedTimestamp);
if (remainingPath != null) {
plannedPaths.add(remainingPath);
plannedAgvIds.put(agv.getAgvId(), "REMAINING_PATH");
@@ -149,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");
@@ -167,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();
@@ -200,7 +237,7 @@
// 瑙勫垝鏃剁┖瀹夊叏鐨勮矾寰�
PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
+ currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
);
if (plannedPath != null) {
@@ -208,7 +245,6 @@
plannedPath.setAgvId(agvId);
plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));
- // 澧炲己璺緞浠ｇ爜淇℃伅
enhancePathCodes(plannedPath, task);
plannedPaths.add(plannedPath);
@@ -225,17 +261,48 @@
}
}
- // 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);
+
+ // 7.6. 閲嶆柊璁＄畻鎵�鏈夎矾寰勭殑鏃堕棿鎴�
+ recalculateAllPathTimings(plannedPaths, agvStatusList, unifiedTimestamp);
+
+ // 8. 鏈�缁堢鎾炴娴嬪拰瑙ｅ喅
System.out.println("姝ラ6: 鏈�缁堢鎾炴娴�");
List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);
@@ -277,9 +344,10 @@
* 澶勭悊CTU鐨勫墿浣欒矾寰�
*
* @param agv CTU鐘舵��
+ * @param unifiedTimestamp 缁熶竴鏃堕棿鎴筹紙绉掞級
* @return 澶勭悊鍚庣殑璺緞
*/
- private PlannedPath processRemainingPath(AGVStatus agv) {
+ private PlannedPath processRemainingPath(AGVStatus agv, long unifiedTimestamp) {
if (!agv.hasRemainingPath()) {
return null;
}
@@ -312,8 +380,8 @@
processedPath.setSegId(segId);
if (timeCalculator != null && !remainingCodes.isEmpty()) {
- // 鑾峰彇AGV鐨勪笅涓�涓矾寰勭偣鍒拌揪鏃堕棿浣滀负璧峰鏃堕棿
- long startTime = agv.getNextPointArrivalTime();
+ // 浣跨敤缁熶竴鏃堕棿鎴充綔涓鸿捣濮嬫椂闂达紙杞崲涓烘绉掞級
+ long startTime = unifiedTimestamp * 1000;
CTUPhysicalConfig config = agv.getPhysicalConfig();
@@ -379,27 +447,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;
}
/**
@@ -488,12 +561,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) {
@@ -511,7 +586,7 @@
if (targetPos != null && !targetPos.equals(currentPos)) {
PlannedPath idlePath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, occupancyMap, agvStatus
+ currentPos, targetPos, constraints, occupancyMap, agvStatus, unifiedTimestamp
);
if (idlePath != null) {
@@ -548,7 +623,6 @@
return;
}
- String actionType = "2"; // 浠诲姟绫诲瀷
String taskId = task.getTaskId();
int backpackLevel = task.getBackpackIndex();
@@ -556,20 +630,23 @@
PathCode pathCode = codeList.get(i);
// 璁剧疆鍩烘湰淇℃伅
- pathCode.setActionType(actionType);
pathCode.setTaskId(taskId);
pathCode.setLev(backpackLevel);
// 璁剧疆鐩爣鐐逛俊鎭�
if (i == codeList.size() - 1) { // 鏈�鍚庝竴涓偣
pathCode.setTargetPoint(true);
+ // 鏍规嵁浠诲姟绫诲瀷璁剧疆actionType鍜宲osType锛�
if (task.isLoaded()) {
+ pathCode.setActionType("2"); // 鏀捐揣
pathCode.setPosType("2"); // 鏀捐揣
} else {
+ pathCode.setActionType("1"); // 鍙栬揣
pathCode.setPosType("1"); // 鍙栬揣
}
} else {
pathCode.setTargetPoint(false);
+ pathCode.setActionType(null);
pathCode.setPosType(null);
}
}
@@ -665,6 +742,111 @@
}
/**
+ * 杩唬妫�鏌ユ墍鏈夌┖闂睞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 + " 杞�");
+ }
+ }
+
+ private void recalculateAllPathTimings(List<PlannedPath> plannedPaths,
+ List<AGVStatus> agvStatusList,
+ long unifiedTimestamp) {
+ if (plannedPaths == null || plannedPaths.isEmpty() || timeCalculator == null) {
+ return;
+ }
+
+ Map<String, AGVStatus> agvStatusMap = new HashMap<>();
+ for (AGVStatus agv : agvStatusList) {
+ if (agv.getAgvId() != null) {
+ agvStatusMap.put(agv.getAgvId(), agv);
+ }
+ }
+
+ long startTimeMs = unifiedTimestamp * 1000;
+ int recalculatedCount = 0;
+
+ // 閬嶅巻鎵�鏈夎矾寰勶紝閲嶆柊璁＄畻鏃堕棿鎴�
+ for (PlannedPath path : plannedPaths) {
+ if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
+ continue;
+ }
+
+ String agvId = path.getAgvId();
+ if (agvId == null) {
+ continue;
+ }
+
+ // 鑾峰彇AGV鐨勭墿鐞嗛厤缃�
+ AGVStatus agvStatus = agvStatusMap.get(agvId);
+ CTUPhysicalConfig config = (agvStatus != null && agvStatus.getPhysicalConfig() != null)
+ ? agvStatus.getPhysicalConfig()
+ : new CTUPhysicalConfig();
+
+ double initialSpeed = 0.0;
+
+ // 閲嶆柊璁＄畻璺緞鏃堕棿鎴�
+ timeCalculator.calculatePathTiming(path, startTimeMs, config, initialSpeed);
+ recalculatedCount++;
+ }
+
+ System.out.println(" 宸查噸鏂拌绠� " + recalculatedCount + " 鏉¤矾寰勭殑鏃堕棿鎴筹紙缁熶竴鏃堕棿鍩哄噯: " + unifiedTimestamp + "绉掞級");
+ }
+
+ /**
* 璇嗗埆闇�瑕佽琛岀殑AGV
* 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤浜嗗叾浠朅GV鍓╀綑璺緞涓婄殑浣嶇疆
*
@@ -729,7 +911,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();
@@ -755,7 +938,7 @@
// 3. 瑙勫垝閬胯璺緞
PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, occupancyMap, yieldAgv
+ currentPos, targetPos, constraints, occupancyMap, yieldAgv, unifiedTimestamp
);
if (yieldPath != null) {
@@ -776,8 +959,15 @@
* 瀵绘壘璁╄鐨勭洰鏍囦綅缃�
*/
private String findYieldTargetPosition(String currentPos, Set<String> blockedPositions, AGVStatus agv) {
- // 浣跨敤BFS鎼滅储鏈�杩戠殑绌洪棽浣嶇疆
- final int MAX_SEARCH_DEPTH = 10;
+ // 浣跨敤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<>(); // 璁板綍浣嶇疆鍜岃窛绂�
@@ -801,19 +991,108 @@
continue;
}
- // 鎵惧埌涓�涓湭琚崰鐢ㄧ殑浣嶇疆
- if (!blockedPositions.contains(neighborPos)) {
- System.out.println(" 鎵惧埌閬胯浣嶇疆: " + neighborPos + " (璺濈=" + (depth + 1) + "姝�)");
+ 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);
- visited.put(neighborPos, depth + 1);
}
}
- System.out.println(" 鏈壘鍒板悎閫傜殑閬胯浣嶇疆 ");
+ 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;
}
@@ -920,7 +1199,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<>());
@@ -941,7 +1221,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);
}
@@ -971,7 +1251,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();
@@ -987,9 +1268,9 @@
return null;
}
- // 瑙勫垝鏃剁┖瀹夊叏鐨勮矾寰�
+ // 瑙勫垝鏃剁┖瀹夊叏鐨勮矾寰勶紙浣跨敤缁熶竴鏃堕棿鎴筹級
PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
- currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
+ currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
);
if (plannedPath != null) {
--
Gitblit v1.9.1