From aaa515911eef34e2263ce8baa6f860025fd6060d Mon Sep 17 00:00:00 2001 From: vincentlu <t1341870251@gmail.com> Date: 星期二, 14 一月 2025 11:21:02 +0800 Subject: [PATCH] # --- zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java | 14 ++- zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java | 16 ++++ zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java | 8 + zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java | 169 ++++++++++++++++++++--------------------- 4 files changed, 112 insertions(+), 95 deletions(-) diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java index 41ea3f3..b7c7104 100644 --- a/zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java +++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java @@ -10,6 +10,22 @@ */ public class MapDataUtils { + public static Double[][][] preComputeCdaMatrix(String[][] cdaStrMatrix) { + int rows = cdaStrMatrix.length; + int cols = cdaStrMatrix[0].length; + + Double[][][] cdaMatrix = new Double[rows][cols][2]; + for (int i = 0; i < rows; i++) { + for (int j = 0; j < cols; j++) { + List<Double> cda = MapDataUtils.parseCdaNode(cdaStrMatrix[i][j]); + cdaMatrix[i][j][0] = cda.get(0); + cdaMatrix[i][j][1] = cda.get(1); + } + } + + return cdaMatrix; + } + public static List<String> parseWaveNode(String waveNodeStr) { List<String> waveNodeList = new ArrayList<>(); if (Cools.isEmpty(waveNodeStr)) { diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java index 08e7042..9fcf125 100644 --- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java +++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java @@ -179,6 +179,7 @@ } private boolean calcWaveScopeByJava(Integer lev) throws Exception { + AgvModel agvModel = agvModelService.selectByType(AgvModelType.CTU_BOX_TRANSPORT_AGV.toString()); // can be optimized Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getDiameter(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR); @@ -187,15 +188,16 @@ String[][] waveMatrix = mapDataDispatcher.initWaveMatrix(lev); // lock path + long time = 0; DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev); for (int i = 0; i < dynamicMatrix.length; i++) { for (int j = 0; j < dynamicMatrix[i].length; j++) { DynamicNode dynamicNode = dynamicMatrix[i][j]; String vehicle = dynamicNode.getVehicle(); if (!DynamicNodeType.ACCESS.val.equals(vehicle) && !DynamicNodeType.BLOCK.val.equals(vehicle)) { - + long startTime = System.currentTimeMillis(); List<NavigateNode> includeList = mapService.getWaveScopeByCode(lev, codeMatrix[i][j], avoidDistance); - + time += System.currentTimeMillis() - startTime; for (NavigateNode navigateNode : includeList) { String waveNode = waveMatrix[navigateNode.getX()][navigateNode.getY()]; // overlay waveMatrix[navigateNode.getX()][navigateNode.getY()] = MapDataUtils.generateWaveNode(waveNode, vehicle); @@ -206,7 +208,7 @@ // mapDataDispatcher.printMatrix(waveMatrix); mapDataDispatcher.setWaveMatrix(lev, waveMatrix); - + System.out.println("calcWaveScopeByJava " + time); return true; } 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 faa379e..2d71496 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 @@ -2,7 +2,6 @@ import com.zy.acs.common.enums.AgvDirectionType; import com.zy.acs.framework.common.Cools; -import com.zy.acs.manager.common.utils.MapDataUtils; import com.zy.acs.manager.core.constant.MapDataConstant; import com.zy.acs.manager.core.domain.SortCodeDto; import com.zy.acs.manager.core.domain.UnlockPathTask; @@ -212,7 +211,7 @@ // v1 BFS ------------------------------------------------------------------------------ public List<NavigateNode> getWaveScopeByCode(Integer lev, String code, Double radiusLen) { String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev); - String[][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev); + Double[][][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev); int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code); NavigateNode originNode = new NavigateNode(codeMatrixIdx[0], codeMatrixIdx[1], code); @@ -229,7 +228,7 @@ } public void spreadWaveNode(NavigateNode originNode, NavigateNode currNode - , String[][] codeMatrix, String[][] cdaMatrix, Double radiusLen + , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLen , List<NavigateNode> includeList, Set<NavigateNode> existNodes) { int x = currNode.getX(); int y = currNode.getY(); @@ -241,7 +240,7 @@ } public void extendNeighborNodes(NavigateNode originNode, NavigateNode nextNode - , String[][] codeMatrix, String[][] cdaMatrix, Double radiusLen + , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLen , List<NavigateNode> includeList, Set<NavigateNode> existNodes) { int x = nextNode.getX(); @@ -258,11 +257,9 @@ existNodes.add(nextNode); - - List<Double> o1Cda = MapDataUtils.parseCdaNode(cdaMatrix[originNode.getX()][originNode.getY()]); - List<Double> o2Cda = MapDataUtils.parseCdaNode(cdaMatrix[x][y]); - - if (Math.pow(o1Cda.get(0) - o2Cda.get(0), 2) + Math.pow(o1Cda.get(1) - o2Cda.get(1), 2) <= Math.pow(radiusLen, 2)) { + 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)) { nextNode.setCodeData(codeMatrix[x][y]); if (!nextNode.getCodeData().equals(CodeNodeType.NONE.val)) { @@ -275,83 +272,83 @@ } // v2 BFS ------------------------------------------------------------------------------ - public List<NavigateNode> getWaveScopeByCode0(Integer lev, String code, Double radiusLen) { - String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev); - - int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code); - NavigateNode originNode = new NavigateNode(codeMatrixIdx[0], codeMatrixIdx[1], code); - - List<NavigateNode> includeList = new ArrayList<>(); - Set<NavigateNode> visited = new HashSet<>(); // Track visited nodes to avoid re-processing - Queue<NavigateNode> queue = new LinkedList<>(); - - includeList.add(originNode); - visited.add(originNode); - queue.offer(originNode); - - while (!queue.isEmpty()) { - NavigateNode currNode = queue.poll(); - this.spreadWaveNode0(originNode, currNode, codeMatrix, radiusLen, includeList, visited, queue); - } - - return includeList; - } - - public void spreadWaveNode0(NavigateNode originNode, NavigateNode currNode, - String[][] codeMatrix, Double radiusLen, - List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) { - - int x = currNode.getX(); - int y = currNode.getY(); - - // Expand neighbors in all four directions (up, down, left, right) - this.extendNeighborNodes0(originNode, new NavigateNode(x, y + 1), codeMatrix, radiusLen, includeList, visited, queue); - this.extendNeighborNodes0(originNode, new NavigateNode(x, y - 1), codeMatrix, radiusLen, includeList, visited, queue); - this.extendNeighborNodes0(originNode, new NavigateNode(x - 1, y), codeMatrix, radiusLen, includeList, visited, queue); - this.extendNeighborNodes0(originNode, new NavigateNode(x + 1, y), codeMatrix, radiusLen, includeList, visited, queue); - } - - public void extendNeighborNodes0(NavigateNode originNode, NavigateNode nextNode, - String[][] codeMatrix, Double radiusLen, - List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) { - int x = nextNode.getX(); - int y = nextNode.getY(); - - // Check if the node is out of bounds - if (x < 0 || x >= codeMatrix.length || y < 0 || y >= codeMatrix[0].length) { - return; - } - - // If the node has already been visited, skip it - if (visited.contains(nextNode)) { - return; - } - - visited.add(nextNode); - - String nextNodeCodeData = codeMatrix[x][y]; - - // If it's a NONE node, we still need to check its surroundings - if (nextNodeCodeData.equals(CodeNodeType.NONE.val)) { - this.spreadWaveNode0(originNode, nextNode, codeMatrix, radiusLen, includeList, visited, queue); - } else { - Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV; - String[][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev); - - // Check if the distance between nodes is within the radius length - List<Double> o1Cda = MapDataUtils.parseCdaNode(cdaMatrix[originNode.getX()][originNode.getY()]); - List<Double> o2Cda = MapDataUtils.parseCdaNode(cdaMatrix[nextNode.getX()][nextNode.getY()]); - - // Calculate Euclidean distance between the nodes - if (Math.pow(o1Cda.get(0) - o2Cda.get(0), 2) + Math.pow(o1Cda.get(1) - o2Cda.get(1), 2) <= Math.pow(radiusLen, 2)) { - nextNode.setCodeData(nextNodeCodeData); - includeList.add(nextNode); - - // Add the node to the queue to expand its neighbors - queue.offer(nextNode); - } - } - } +// public List<NavigateNode> getWaveScopeByCode0(Integer lev, String code, Double radiusLen) { +// String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev); +// +// int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code); +// NavigateNode originNode = new NavigateNode(codeMatrixIdx[0], codeMatrixIdx[1], code); +// +// List<NavigateNode> includeList = new ArrayList<>(); +// Set<NavigateNode> visited = new HashSet<>(); // Track visited nodes to avoid re-processing +// Queue<NavigateNode> queue = new LinkedList<>(); +// +// includeList.add(originNode); +// visited.add(originNode); +// queue.offer(originNode); +// +// while (!queue.isEmpty()) { +// NavigateNode currNode = queue.poll(); +// this.spreadWaveNode0(originNode, currNode, codeMatrix, radiusLen, includeList, visited, queue); +// } +// +// return includeList; +// } +// +// public void spreadWaveNode0(NavigateNode originNode, NavigateNode currNode, +// String[][] codeMatrix, Double radiusLen, +// List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) { +// +// int x = currNode.getX(); +// int y = currNode.getY(); +// +// // Expand neighbors in all four directions (up, down, left, right) +// this.extendNeighborNodes0(originNode, new NavigateNode(x, y + 1), codeMatrix, radiusLen, includeList, visited, queue); +// this.extendNeighborNodes0(originNode, new NavigateNode(x, y - 1), codeMatrix, radiusLen, includeList, visited, queue); +// this.extendNeighborNodes0(originNode, new NavigateNode(x - 1, y), codeMatrix, radiusLen, includeList, visited, queue); +// this.extendNeighborNodes0(originNode, new NavigateNode(x + 1, y), codeMatrix, radiusLen, includeList, visited, queue); +// } +// +// public void extendNeighborNodes0(NavigateNode originNode, NavigateNode nextNode, +// String[][] codeMatrix, Double radiusLen, +// List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) { +// int x = nextNode.getX(); +// int y = nextNode.getY(); +// +// // Check if the node is out of bounds +// if (x < 0 || x >= codeMatrix.length || y < 0 || y >= codeMatrix[0].length) { +// return; +// } +// +// // If the node has already been visited, skip it +// if (visited.contains(nextNode)) { +// return; +// } +// +// visited.add(nextNode); +// +// String nextNodeCodeData = codeMatrix[x][y]; +// +// // If it's a NONE node, we still need to check its surroundings +// if (nextNodeCodeData.equals(CodeNodeType.NONE.val)) { +// this.spreadWaveNode0(originNode, nextNode, codeMatrix, radiusLen, includeList, visited, queue); +// } else { +// Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV; +// String[][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev); +// +// // Check if the distance between nodes is within the radius length +// List<Double> o1Cda = MapDataUtils.parseCdaNode(cdaMatrix[originNode.getX()][originNode.getY()]); +// List<Double> o2Cda = MapDataUtils.parseCdaNode(cdaMatrix[nextNode.getX()][nextNode.getY()]); +// +// // Calculate Euclidean distance between the nodes +// if (Math.pow(o1Cda.get(0) - o2Cda.get(0), 2) + Math.pow(o1Cda.get(1) - o2Cda.get(1), 2) <= Math.pow(radiusLen, 2)) { +// nextNode.setCodeData(nextNodeCodeData); +// includeList.add(nextNode); +// +// // Add the node to the queue to expand its neighbors +// queue.offer(nextNode); +// } +// } +// } public Boolean isTurnCorner(String codeData) { diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java index 72b5182..04be5ac 100644 --- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java +++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java @@ -6,6 +6,7 @@ import com.zy.acs.common.utils.RedisSupport; import com.zy.acs.framework.common.Cools; import com.zy.acs.framework.exception.CoolException; +import com.zy.acs.manager.common.utils.MapDataUtils; import com.zy.acs.manager.core.service.astart.domain.DynamicNode; import com.zy.acs.manager.core.utils.RouteGenerator; import com.zy.acs.manager.manager.entity.Code; @@ -36,7 +37,7 @@ private int[][] turnMatrix; - private String[][] cdaMatrix; + private Double[][][] cdaMatrix; public Map<String, Boolean> routeCdaMap = new HashMap<>(); @@ -89,13 +90,14 @@ redis.setValue(RedisConstant.AGV_MAP_ASTAR_DYNAMIC_FLAG, String.valueOf(lev), JSON.toJSONString(dynamicMatrix)); } - public String[][] getCdaMatrix(Integer lev) { + public Double[][][] getCdaMatrix(Integer lev) { lev = Optional.ofNullable(lev).orElse(MAP_DEFAULT_LEV); // redis if (null == this.cdaMatrix) { String cdaMatrixStr = redis.getValue(RedisConstant.AGV_MAP_ASTAR_CDA_FLAG, String.valueOf(lev)); if (!Cools.isEmpty(cdaMatrixStr)) { - this.cdaMatrix = JSON.parseObject(cdaMatrixStr, String[][].class); + String[][] cdaStrMatrix = JSON.parseObject(cdaMatrixStr, String[][].class); + this.cdaMatrix = MapDataUtils.preComputeCdaMatrix(cdaStrMatrix); } } // init @@ -110,9 +112,9 @@ return this.cdaMatrix; } - public void setCdaMatrix(Integer lev, String[][] cdaMatrix) { - redis.setValue(RedisConstant.AGV_MAP_ASTAR_CDA_FLAG, String.valueOf(lev), JSON.toJSONString(cdaMatrix)); - this.cdaMatrix = cdaMatrix; + public void setCdaMatrix(Integer lev, String[][] cdaStrMatrix) { + redis.setValue(RedisConstant.AGV_MAP_ASTAR_CDA_FLAG, String.valueOf(lev), JSON.toJSONString(cdaStrMatrix)); + this.cdaMatrix = MapDataUtils.preComputeCdaMatrix(cdaStrMatrix); } public int[][] getTurnMatrix(Integer lev) { -- Gitblit v1.9.1