From a079fff1e7bae25a5e29f73709b8e9972216f7ef Mon Sep 17 00:00:00 2001
From: vincentlu <t1341870251@gmail.com>
Date: 星期二, 21 一月 2025 15:20:29 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java | 58 +++++++++++++++++++++++++++++++++++++++++++++++++---------
1 files changed, 49 insertions(+), 9 deletions(-)
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
index 2d71496..1f0817d 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
@@ -183,8 +183,46 @@
}
public void unlockPath(String agvNo, String codeData) {
+ if (Cools.isEmpty(agvNo, codeData)) {
+ return;
+ }
try {
unlockTaskQueue.offer(new UnlockPathTask(agvNo, codeData), 5, TimeUnit.SECONDS);
+
+// Integer lev = null;
+//
+// String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
+// int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, codeData);
+//
+//
+// DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
+//
+// DynamicNode dynamicNode = dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
+//
+//
+// int serial = dynamicNode.getSerial();
+//
+// List<int[]> resetCodeIdxList = new ArrayList<>();
+//
+// for (int i = 0; i < dynamicMatrix.length; i++) {
+// for (int j = 0; j < dynamicMatrix[i].length; j++) {
+//
+//// if (i == codeMatrixIdx[0] && j == codeMatrixIdx[1]) { continue; }
+//
+// DynamicNode node = dynamicMatrix[i][j];
+// if (node.getVehicle().equals(agvNo)) {
+// if (node.getSerial() < serial) {
+// resetCodeIdxList.add(new int[] {i, j});
+// }
+// }
+// }
+// }
+//
+// if (!Cools.isEmpty(resetCodeIdxList)) {
+//
+// mapDataDispatcher.clearDynamicMatrixByCodeList(lev, resetCodeIdxList);
+// }
+
} catch (InterruptedException e) {
log.error("unlockTaskQueue", e);
}
@@ -210,6 +248,8 @@
// v1 BFS ------------------------------------------------------------------------------
public List<NavigateNode> getWaveScopeByCode(Integer lev, String code, Double radiusLen) {
+ double radiusLenSquared = Math.pow(radiusLen, 2);
+
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
Double[][][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
@@ -222,25 +262,25 @@
includeList.add(originNode);
existNodes.add(originNode);
- this.spreadWaveNode(originNode, originNode, codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
+ this.spreadWaveNode(originNode, originNode, codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
return includeList;
}
public void spreadWaveNode(NavigateNode originNode, NavigateNode currNode
- , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLen
+ , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLenSquared
, List<NavigateNode> includeList, Set<NavigateNode> existNodes) {
int x = currNode.getX();
int y = currNode.getY();
- this.extendNeighborNodes(originNode, new NavigateNode(x, y + 1), codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
- this.extendNeighborNodes(originNode, new NavigateNode(x, y - 1), codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
- this.extendNeighborNodes(originNode, new NavigateNode(x - 1, y), codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
- this.extendNeighborNodes(originNode, new NavigateNode(x + 1, y), codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
+ this.extendNeighborNodes(originNode, new NavigateNode(x, y + 1), codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
+ this.extendNeighborNodes(originNode, new NavigateNode(x, y - 1), codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
+ this.extendNeighborNodes(originNode, new NavigateNode(x - 1, y), codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
+ this.extendNeighborNodes(originNode, new NavigateNode(x + 1, y), codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
}
public void extendNeighborNodes(NavigateNode originNode, NavigateNode nextNode
- , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLen
+ , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLenSquared
, List<NavigateNode> includeList, Set<NavigateNode> existNodes) {
int x = nextNode.getX();
@@ -259,14 +299,14 @@
Double[] o1Cda = cdaMatrix[originNode.getX()][originNode.getY()];
Double[] o2Cda = cdaMatrix[x][y];
- if (Math.pow(o1Cda[0] - o2Cda[0], 2) + Math.pow(o1Cda[1] - o2Cda[1], 2) <= Math.pow(radiusLen, 2)) {
+ if (Math.pow(o1Cda[0] - o2Cda[0], 2) + Math.pow(o1Cda[1] - o2Cda[1], 2) <= radiusLenSquared) {
nextNode.setCodeData(codeMatrix[x][y]);
if (!nextNode.getCodeData().equals(CodeNodeType.NONE.val)) {
includeList.add(nextNode);
}
- this.spreadWaveNode(originNode, nextNode, codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
+ this.spreadWaveNode(originNode, nextNode, codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
}
}
--
Gitblit v1.9.1