From cd67e854d4715bc92a16dc7bffdbde7be1da9957 Mon Sep 17 00:00:00 2001
From: vincentlu <t1341870251@gmail.com>
Date: 星期四, 27 三月 2025 13:02:37 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java | 72 +++++++++++++++++++++++++++++++-----
1 files changed, 62 insertions(+), 10 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..cb00614 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
@@ -123,6 +123,18 @@
}).collect(Collectors.toList());
}
+ public static Double mapToNearest(Double angle) {
+ if (null == angle) {
+ return null;
+ }
+ angle = (angle + 360) % 360;
+ double remainder = angle % 45;
+ if (remainder < 22.5) {
+ return angle - remainder;
+ } else {
+ return angle + (45 - remainder);
+ }
+ }
// 瑙掑害璁＄畻
public Double calculateDirection(Code startCode, Code endCode, int angleOffsetVal) {
@@ -134,7 +146,7 @@
double deltaX = x1 - x0;
double deltaY = y1 - y0;
- double angle = -Math.atan2(deltaX, deltaY);
+ double angle = Math.atan2(deltaX, deltaY);
angle = Math.toDegrees(angle) + angleOffsetVal;
angle = (angle + 360) % 360; // 灏嗚搴﹁浆鎹负姝ｅ��
@@ -183,8 +195,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 +260,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 +274,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 +311,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