From 7ac5a3f2bcbb0d0e13dc87cd969c8fb1fd99b488 Mon Sep 17 00:00:00 2001
From: vincentlu <t1341870251@gmail.com>
Date: 星期日, 04 一月 2026 10:38:42 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java | 100 ++++++++++++++++++++++++++++++++++++++-----------
1 files changed, 77 insertions(+), 23 deletions(-)
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
index b8e0a90..81d9679 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
@@ -3,11 +3,13 @@
import com.zy.acs.common.utils.RedisSupport;
import com.zy.acs.framework.common.Cools;
import com.zy.acs.manager.common.utils.MapDataUtils;
+import com.zy.acs.manager.core.service.AgvAreaDispatcher;
import com.zy.acs.manager.core.service.LaneService;
import com.zy.acs.manager.core.service.astart.domain.AStarNavigateNode;
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.Segment;
+import com.zy.acs.manager.manager.service.AgvService;
import com.zy.acs.manager.manager.service.JamService;
import com.zy.acs.manager.system.service.ConfigService;
import lombok.extern.slf4j.Slf4j;
@@ -15,7 +17,6 @@
import org.springframework.stereotype.Service;
import java.util.*;
-import java.util.concurrent.CopyOnWriteArrayList;
/**
* Created by vincent on 6/12/2024
@@ -30,6 +31,9 @@
public static final int WEIGHT_CALC_FACTOR = 1;
+ // right left up down
+ private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};
+
@Autowired
private MapDataDispatcher mapDataDispatcher;
@Autowired
@@ -38,16 +42,39 @@
private LaneService laneService;
@Autowired
private ConfigService configService;
+ @Autowired
+ private AgvAreaDispatcher agvAreaDispatcher;
+ @Autowired
+ private AgvService agvService;
public synchronized AStarNavigateNode execute(String agvNo, AStarNavigateNode start, AStarNavigateNode end
, Boolean lock, List<String> blackList, Segment segment) {
if (start.getX() == end.getX() && start.getY() == end.getY()) {
return end;
}
+
+ // scope code area: 4ms
+ Long agvId = agvService.getAgvId(agvNo);
+ Boolean withinArea = agvAreaDispatcher.isAgvExistsInAnyArea(agvId);
+ List<String> scopeCodeList = new ArrayList<>();
+ if (withinArea) {
+ scopeCodeList = agvAreaDispatcher.getCodesByAgvId(agvId);
+ if (!Cools.isEmpty(scopeCodeList) && !scopeCodeList.contains(start.getCodeData())) {
+ withinArea = false;
+ }
+ }
+
Integer maxAgvCountInLane = configService.getVal("maxAgvCountInLane", Integer.class);
PriorityQueue<AStarNavigateNode> openQueue = new PriorityQueue<>();
Set<AStarNavigateNode> existNodes = new HashSet<>();
+ Map<String, Integer> bestGMap = new HashMap<>();
+
+ start.setG(0);
+ start.setH(calcNodeCost(start, end));
+ start.setF(start.getG() + start.getH());
+ String startKey = start.getX() + "_" + start.getY();
+ bestGMap.put(startKey, start.getG());
openQueue.add(start);
existNodes.add(start);
@@ -56,17 +83,25 @@
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(null);
String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(null);
+
while (!openQueue.isEmpty()) {
// 鍙栦紭鍏堥槦鍒楅《閮ㄥ厓绱犲苟涓旀妸杩欎釜鍏冪礌浠嶰pen琛ㄤ腑鍒犻櫎锛屽彇F鍊兼渶灏忕殑鑺傜偣
AStarNavigateNode currentNode = openQueue.poll();
// 缁堢偣
if (currentNode.getX() == end.getX() && currentNode.getY() == end.getY()) {
+// System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
return currentNode;
}
+ long currentTime = System.currentTimeMillis();
List<AStarNavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
for (AStarNavigateNode node : neighbourNodes) {
node.setCodeData(codeMatrix[node.getX()][node.getY()]);
+
+ if (withinArea) {
+ assert !Cools.isEmpty(scopeCodeList);
+ if (!scopeCodeList.contains(node.getCodeData())) { continue; }
+ }
boolean isEndNode = node.getX() == end.getX() && node.getY() == end.getY();
@@ -90,6 +125,9 @@
assert !vehicle.equals(DynamicNodeType.BLOCK.val);
if (!vehicle.equals(DynamicNodeType.ACCESS.val)) {
if (!vehicle.equals(agvNo)) {
+ if (lock) {
+ continue;
+ }
// 濡傛灉瀛樺湪杞﹁締锛屽垯澧炲姞鏉冮噸 2 鎴栬�� 3锛屽洜涓烘嫄鐐逛細澧炲姞鏉冮噸 1
// vehicle宸茬粡涓哄綋鍓峴egment鍋氳繃浜嗛伩璁╋紝涓旈伩璁╀换鍔″凡瀹屾垚锛屽垯鏉冮噸鍊煎鍔�
@@ -100,10 +138,6 @@
weight += (WEIGHT_CALC_FACTOR * 2);
}
}
-
- if (lock) {
- continue;
- }
}
}
@@ -113,7 +147,6 @@
if (!waveNode.equals(WaveNodeType.ENABLE.val)) {
List<String> waveNodeList = MapDataUtils.parseWaveNode(waveNode);
List<String> otherWaveList = MapDataUtils.hasOtherWave(waveNodeList, agvNo);
-
if (!Cools.isEmpty(otherWaveList)) {
if (lock) {
@@ -159,11 +192,19 @@
node.setH(calcNodeCost(node, end));
node.setF(node.getG() + node.getH());
- openQueue.add(node);
- existNodes.add(node);
+ String key = node.getX() + "_" + node.getY();
+ Integer recordedG = bestGMap.get(key);
+ if (recordedG == null || node.getG() <= recordedG) {
+ bestGMap.put(key, node.getG());
+
+ openQueue.add(node);
+ }
+
+// openQueue.add(node);
+// existNodes.add(node);
}
}
-
+// System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
return null;
}
@@ -171,20 +212,33 @@
private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes) {
int x = currentNode.getX();
int y = currentNode.getY();
+ AStarNavigateNode parent = currentNode.getParent();
- List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
+// List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
+ List<AStarNavigateNode> neighbourNodes = new ArrayList<>();
- List<AStarNavigateNode> possibleNodes = Arrays.asList(
- new AStarNavigateNode(x, y + 1), // right
- new AStarNavigateNode(x, y - 1), // left
- new AStarNavigateNode(x - 1, y), // up
- new AStarNavigateNode(x + 1, y) // down
- );
+ List<AStarNavigateNode> possibleNodes = new ArrayList<>();
+ for (int[] d: DIRECTIONS) {
+ int nx = x + d[0];
+ int ny = y + d[1];
+ // 濡傛灉鐖惰妭鐐逛笉涓虹┖锛屽苟涓� (nx,ny) 绛変簬鐖惰妭鐐瑰潗鏍囷紝鍒欒烦杩�
+ if (parent != null && nx == parent.getX() && ny == parent.getY()) {
+ continue;
+ }
+ possibleNodes.add(new AStarNavigateNode(nx, ny));
+ }
- possibleNodes.parallelStream()
- .map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
- .filter(Objects::nonNull)
- .forEach(neighbourNodes::add);
+// possibleNodes.parallelStream()
+// .map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
+// .filter(Objects::nonNull)
+// .forEach(neighbourNodes::add);
+
+ for (AStarNavigateNode pn : possibleNodes) {
+ AStarNavigateNode next = extendNeighborNodes(currentNode, pn, mapMatrix, existNodes, null, null);
+ if (next != null) {
+ neighbourNodes.add(next);
+ }
+ }
return neighbourNodes;
}
@@ -215,9 +269,9 @@
assert mapMatrix[x][y] == MapNodeType.ENABLE.val;
- if (existNodes.contains(nextNode)) {
- return null;
- }
+// if (existNodes.contains(nextNode)) {
+// return null;
+// }
// 鍒ゆ柇閫氳繃鎬�
String routeCdaKey = RouteGenerator.generateRouteCdaKey(new int[]{currentNode.getX(), currentNode.getY()}, new int[]{nextNode.getX(), nextNode.getY()});
--
Gitblit v1.9.1