From ebd2f4397a92c6a5096de1b86d59154363344720 Mon Sep 17 00:00:00 2001
From: vincentlu <t1341870251@gmail.com>
Date: 星期二, 13 五月 2025 08:48:15 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java | 326 ++++++++++++++++++++++++------------------------------
1 files changed, 146 insertions(+), 180 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 0ebf382..a7ea660 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,16 +3,14 @@
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.domain.Lane;
-import com.zy.acs.manager.core.domain.type.BlockSeverityType;
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.manager.entity.Route;
+import com.zy.acs.manager.core.utils.RouteGenerator;
import com.zy.acs.manager.manager.entity.Segment;
-import com.zy.acs.manager.manager.service.CodeService;
import com.zy.acs.manager.manager.service.JamService;
-import com.zy.acs.manager.manager.service.RouteService;
import com.zy.acs.manager.system.service.ConfigService;
+import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;
@@ -21,19 +19,19 @@
/**
* Created by vincent on 6/12/2024
*/
+@Slf4j
@Service
public class AStarNavigateService {
private final RedisSupport redis = RedisSupport.defaultRedisSupport;
- public static final boolean OPEN_TURN_COST_WEIGHT = Boolean.FALSE;
+ public static final boolean OPEN_TURN_COST_WEIGHT = Boolean.TRUE;
public static final int WEIGHT_CALC_FACTOR = 1;
- @Autowired
- private CodeService codeService;
- @Autowired
- private RouteService routeService;
+ // right left up down
+ private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};
+
@Autowired
private MapDataDispatcher mapDataDispatcher;
@Autowired
@@ -43,35 +41,64 @@
@Autowired
private ConfigService configService;
- public synchronized NavigateNode execute(String agvNo, NavigateNode start, NavigateNode end
- , Boolean lock, List<String> blackList, Segment segment, BlockSeverityType blockSeverity) {
+ 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;
}
Integer maxAgvCountInLane = configService.getVal("maxAgvCountInLane", Integer.class);
- PriorityQueue<NavigateNode> openQueue = new PriorityQueue<>();
- ArrayList<NavigateNode> existNodes = new ArrayList<>();
+ 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);
int[][] mapMatrix = mapDataDispatcher.getMapMatrix(null, null);
+ String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(null);
String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(null);
- while (openQueue.size() > 0) {
- // 鍙栦紭鍏堥槦鍒楅《閮ㄥ厓绱犲苟涓旀妸杩欎釜鍏冪礌浠嶰pen琛ㄤ腑鍒犻櫎锛屽彇F鍊兼渶灏忕殑鑺傜偣
- NavigateNode currentNode = openQueue.poll();
+ long getNeighborNodesTime = 0;
+ int getNeighborNodesCount = 0;
- ArrayList<NavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
- for (NavigateNode node : neighbourNodes) {
+ 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);
+ getNeighborNodesTime += System.currentTimeMillis() - currentTime;
+ getNeighborNodesCount ++;
+ for (AStarNavigateNode node : neighbourNodes) {
+ node.setCodeData(codeMatrix[node.getX()][node.getY()]);
+
boolean isEndNode = node.getX() == end.getX() && node.getY() == end.getY();
int weight = 0;
if (!Cools.isEmpty(blackList) && blackList.contains(node.getCodeData())) {
continue;
+ }
+ // 鐗规畩鎯呭喌锛屽綋blackList鏈変笖鍙湁涓�涓厓绱犱笖涓簊tartNode鏃�
+ // 璇存槑blackList宸茬粡鐭ラ亾褰撳墠瀵艰埅璧峰鐐瑰拰鐩爣鐐逛负鐩搁偦鑺傜偣
+ // 浣嗘槸褰撳墠blackList鐨勪换鍔℃槸涓嶈绯荤粺璧扮浉閭荤殑鏈�鐭矾寰勶紝鎵�浠ユ墠浼氭湁涓嬮潰鐨勫垽鏂拰continue
+ if (blackList.size() == 1 && blackList.get(0).equals(start.getCodeData())) {
+ if (isEndNode && currentNode.getCodeData().equals(start.getCodeData())) {
+ continue;
+ }
}
// 鑺傜偣琚崰鐢�
@@ -80,16 +107,18 @@
assert !vehicle.equals(DynamicNodeType.BLOCK.val);
if (!vehicle.equals(DynamicNodeType.ACCESS.val)) {
if (!vehicle.equals(agvNo)) {
-
- // 瀛樺湪杞﹁締锛屼笖涓哄凡缁忛伩璁╃殑杞︼紝鍒欐潈閲嶅�煎鍔�
- if (null != segment) {
- if (!Cools.isEmpty(jamService.getJamFromSegmentByAvo(segment, vehicle))) {
- weight += WEIGHT_CALC_FACTOR;
- }
- }
-
if (lock) {
continue;
+ }
+
+ // 濡傛灉瀛樺湪杞﹁締锛屽垯澧炲姞鏉冮噸 2 鎴栬�� 3锛屽洜涓烘嫄鐐逛細澧炲姞鏉冮噸 1
+ // vehicle宸茬粡涓哄綋鍓峴egment鍋氳繃浜嗛伩璁╋紝涓旈伩璁╀换鍔″凡瀹屾垚锛屽垯鏉冮噸鍊煎鍔�
+ if (null != segment) {
+ if (!Cools.isEmpty(jamService.getJamFromSegmentByAvo(segment, vehicle))) {
+ weight += (WEIGHT_CALC_FACTOR * 3);
+ } else {
+ weight += (WEIGHT_CALC_FACTOR * 2);
+ }
}
}
}
@@ -100,7 +129,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) {
@@ -110,96 +138,102 @@
}
// 鍗曞贩閬撹溅杈嗗杞芥暟閲�
- Lane lane = laneService.search(node.getCodeData());
- if (null != lane) {
+ List<int[]> laneCodeIdxList = laneService.getLaneCodeIdxList(node.getCodeData());
+ if (!Cools.isEmpty(laneCodeIdxList)) {
Set<String> lanVehicleSet = new HashSet<>();
- List<String> laneCodes = lane.getCodes();
- for (String laneCodeData : laneCodes) {
- int[] laneCodeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(null, laneCodeData);
- // scan dynamicMatrix or WaveMatrix
- DynamicNode laneDynamicNode = dynamicMatrix[laneCodeMatrixIdx[0]][laneCodeMatrixIdx[1]];
+ for (int[] codeMatrixIdx : laneCodeIdxList) {
+ DynamicNode laneDynamicNode = dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
String laneVehicle = laneDynamicNode.getVehicle();
assert !laneVehicle.equals(DynamicNodeType.BLOCK.val);
if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) {
if (!laneVehicle.equals(agvNo)) {
lanVehicleSet.add(laneVehicle);
-// redis.setObject(RedisConstant.AGV_TO_STANDBY_FLAG, laneVehicle, true, 30);
}
}
}
-
if (lanVehicleSet.size() + 1 > maxAgvCountInLane) {
continue;
}
}
-
- //鎵惧埌鐩爣缁撶偣灏辫繑鍥�
- if (isEndNode) {
- //骞朵笖璁＄畻鍑篏锛� F锛� H绛夊��
- node.initNode(currentNode, end);
- return node;
+ if (OPEN_TURN_COST_WEIGHT) {
+ if (this.isTurning(currentNode, node)) {
+ weight += WEIGHT_CALC_FACTOR;
+ node.setTurnCount(currentNode.getTurnCount() + 1);
+ } else {
+ // 鏂瑰悜娌″彉
+ node.setTurnCount(currentNode.getTurnCount());
+ }
}
-
- // G + H + T (瀵瑰惎鍙戝嚱鏁板鍔犲幓鎷愮偣鏂规calcNodeTurnCost)
- int gCost = calcNodeCost(currentNode, node) * (OPEN_TURN_COST_WEIGHT ? calcNodeTurnCost(currentNode, node, end) : 1);
//杩涜璁＄畻瀵� G, F, H 绛夊��
node.setWeight(weight);
- node.setLastDistance(gCost);
+ node.setLastDistance(calcNodeCost(currentNode, node));
node.initNode(currentNode, end);
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);
}
}
- //濡傛灉閬嶅巻瀹屾墍鏈夊嚭鐜扮殑缁撶偣閮芥病鏈夋壘鍒版渶缁堢殑缁撶偣锛岃繑鍥瀗ull
+// System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
return null;
}
// 鑾峰彇鍥涘懆鑺傜偣
- private ArrayList<NavigateNode> getNeighborNodes(NavigateNode currentNode, int[][] mapMatrix, List<NavigateNode> existNodes) {
- //鑾峰彇褰撳墠缁撶偣鐨剎, y
+ private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes) {
int x = currentNode.getX();
int y = currentNode.getY();
- //濡傛灉褰撳墠缁撶偣鐨勯偦缁撶偣鍚堟硶锛屽氨鍔犲叆鍒皀eighbour_node
- ArrayList<NavigateNode> neighbourNodes = new ArrayList<>();
+ AStarNavigateNode parent = currentNode.getParent();
- NavigateNode rightNode = extendNeighborNodes(currentNode, new NavigateNode(x, y + 1), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, rightNode, mapMatrix, existNodes)) {
- neighbourNodes.add(rightNode);
+// List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
+ List<AStarNavigateNode> neighbourNodes = new ArrayList<>();
+
+ 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));
}
- NavigateNode leftNode = extendNeighborNodes(currentNode, new NavigateNode(x, y - 1), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, leftNode, mapMatrix, existNodes)) {
- neighbourNodes.add(leftNode);
- }
+// possibleNodes.parallelStream()
+// .map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
+// .filter(Objects::nonNull)
+// .forEach(neighbourNodes::add);
- NavigateNode topNode = extendNeighborNodes(currentNode, new NavigateNode(x - 1, y), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, topNode, mapMatrix, existNodes)) {
- neighbourNodes.add(topNode);
- }
-
- NavigateNode bottomNode = extendNeighborNodes(currentNode, new NavigateNode(x + 1, y), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, bottomNode, mapMatrix, existNodes)) {
- neighbourNodes.add(bottomNode);
+ for (AStarNavigateNode pn : possibleNodes) {
+ AStarNavigateNode next = extendNeighborNodes(currentNode, pn, mapMatrix, existNodes, null, null);
+ if (next != null) {
+ neighbourNodes.add(next);
+ }
}
return neighbourNodes;
}
- private NavigateNode extendNeighborNodes(NavigateNode currentNode, NavigateNode extendNode, int[][] mapMatrix, List<NavigateNode> existNodes, Integer dx, Integer dy) {
- NavigateNode nextNode = null;
+ private AStarNavigateNode extendNeighborNodes(AStarNavigateNode currentNode, AStarNavigateNode extendNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes, Integer dx, Integer dy) {
+ AStarNavigateNode nextNode;
if (null == dx || null == dy) {
dx = extendNode.getX() - currentNode.getX();
dy = extendNode.getY() - currentNode.getY();
nextNode = extendNode;
} else {
- nextNode = new NavigateNode(extendNode.getX() + dx, extendNode.getY() + dy);
+ nextNode = new AStarNavigateNode(extendNode.getX() + dx, extendNode.getY() + dy);
}
int x = nextNode.getX();
@@ -213,129 +247,61 @@
if (mapMatrix[x][y] == MapNodeType.DISABLE.val) {
return extendNeighborNodes(currentNode, nextNode, mapMatrix, existNodes, dx, dy);
-
- } else {
-
- if (isExist(nextNode, existNodes)) {
- return null;
- }
-
- // 鑺傜偣鏄惁鍙敤
- if (mapMatrix[x][y] != MapNodeType.ENABLE.val) {
- return null;
- }
-
- String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
- String currentNodeCodeData = codeMatrix[currentNode.getX()][currentNode.getY()];
- String nextNodeCodeData = codeMatrix[nextNode.getX()][nextNode.getY()];
- nextNode.setCodeData(nextNodeCodeData);
-
- // 鍒ゆ柇閫氳繃鎬�
- Route route = routeService.findByCodeOfBoth(
- codeService.selectByData(currentNodeCodeData).getId(),
- codeService.selectByData(nextNodeCodeData).getId()
- );
- if (null == route) {
- return null;
- }
-
- return nextNode;
}
- }
- private boolean is_valid(NavigateNode currentNode, NavigateNode node, int[][] mapMatrix, List<NavigateNode> existNodes) {
- if (null == node) {
- return false;
- }
-// int x = node.getX();
-// int y = node.getY();
-// if (x < 0 || x >= mapMatrix.length
-// || y < 0 || y >= mapMatrix[0].length) {
-// return false;
-// }
-//
-// // 濡傛灉缁撶偣鐨勪綅缃皬浜�0锛屽垯涓嶅悎娉�
-// if (mapMatrix[x][y] < 0) return false;
-//
-// if (is_exist(node, existNodes)) {
-// return false;
-// }
-//
-// // 鍒ゆ柇閫氳繃鎬�
-// String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
-// String currentNodeCodeData = codeMatrix[currentNode.getX()][currentNode.getY()];
-// String nextNodeCodeData = codeMatrix[node.getX()][node.getY()];
-// node.setCodeData(nextNodeCodeData);
-//
-// Route route = routeService.findByCodeOfBoth(
-// codeService.selectByData(currentNodeCodeData).getId(),
-// codeService.selectByData(nextNodeCodeData).getId()
-// );
-// if (null == route) {
-// return false;
+ assert mapMatrix[x][y] == MapNodeType.ENABLE.val;
+
+// if (existNodes.contains(nextNode)) {
+// return null;
// }
- return true;
- }
-
-// private boolean is_exist(NavigateNode node, List<NavigateNode> existNodes) {
-// for (NavigateNode exist_node : existNodes) {
-// if (node.getX() == exist_node.getX() && node.getY() == exist_node.getY()) {
-// return true;
-// }
-// }
-// return false;
-// }
-
- private boolean isExist(NavigateNode node, List<NavigateNode> existNodes) {
- for (NavigateNode existNode : existNodes) {
- if (this.isSame(node, existNode)) {
- return true;
- }
+ // 鍒ゆ柇閫氳繃鎬�
+ String routeCdaKey = RouteGenerator.generateRouteCdaKey(new int[]{currentNode.getX(), currentNode.getY()}, new int[]{nextNode.getX(), nextNode.getY()});
+ if (!mapDataDispatcher.validRouteCdaKey(routeCdaKey)) {
+ return null;
}
- return false;
- }
- private boolean isSame(NavigateNode o1, NavigateNode o2) {
- if (Cools.isEmpty(o1, o2)) {
- return false;
- }
- return o1.getX() == o2.getX() && o1.getY() == o2.getY();
+ return nextNode;
}
//------------------A*鍚彂鍑芥暟------------------//
//璁＄畻閫氳繃鐜板湪鐨勭粨鐐圭殑浣嶇疆鍜屾渶缁堢粨鐐圭殑浣嶇疆璁＄畻H鍊�(鏇煎搱椤挎硶锛氬潗鏍囧垎鍒彇宸�肩浉鍔�)
- private int calcNodeCost(NavigateNode node1, NavigateNode node2) {
-// Code code1 = codeService.selectByData(node1.getCodeData());
-// Code code2 = codeService.selectByData(node2.getCodeData());
-// return (int) (Math.abs(code2.getX() - code1.getX()) + Math.abs(code2.getY() - code1.getY()));
-
+ private int calcNodeCost(AStarNavigateNode node1, AStarNavigateNode node2) {
return Math.abs(node2.getX() - node1.getX()) + Math.abs(node2.getY() - node1.getY());
}
- //鍘婚櫎鎷愮偣绠楁硶锛岀粰鐩寸嚎澧炲姞浼樺厛绾�
- private int calcNodeTurnCost(NavigateNode currNode, NavigateNode nextNode, NavigateNode endNode) {
- // 绗竴涓偣鎴栫洿绾跨偣
- if (currNode.getParent() == null
- || nextNode.getX() == currNode.getParent().getX()
- || nextNode.getY() == currNode.getParent().getY()
- ) {
- return 1;
+ // 杞集鍒ゆ柇锛氬彧鍏佽鈥滃瀭鐩存垨姘村钩鈥濊繍鍔�
+ private boolean isTurning(AStarNavigateNode currNode, AStarNavigateNode nextNode) {
+ // 绗竴涓偣
+ if (currNode.getParent() == null) {
+ return false;
}
-
- // 鎷愬悜缁堢偣鐨勭偣
- if (nextNode.getX() == endNode.getX() || nextNode.getY() == endNode.getY()) {
- return 2;
- }
-
- // 鏅�氭嫄鐐�
- /*
- 鎷愮偣鍒ゆ柇閫昏緫
- 鎷垮埌鐖惰妭鐐瑰拰涓嬩竴鑺傜偣
- 閫氳繃鍒ゆ柇鐖惰妭鐐瑰拰涓嬩竴鑺傜偣鐨剎鏁版嵁鍜寉鏁版嵁閮戒笉鐩稿悓鏃讹紝鍒欒〃鏄庡綋鍓嶅潗鏍囨槸涓�涓嫄鐐�
- */
- return 3;
+ AStarNavigateNode parent = currNode.getParent();
+ // 濡傛灉涓嬩竴鐐癸紙nextNode锛変笌 parent 鍦ㄥ悓涓�琛屾垨鍚屼竴鍒� => 娌℃湁杞集
+ // 娉ㄦ剰锛岃繖瀹為檯涓婄瓑鍚屼簬鈥�(curr->next) 鐨勬柟鍚� == (parent->curr) 鐨勬柟鍚戔�濄��
+ boolean sameRowOrCol =
+ (nextNode.getX() == parent.getX())
+ || (nextNode.getY() == parent.getY());
+ return !sameRowOrCol;
}
+ // 杞集鍒ゆ柇锛氬鏋滆繖涓や釜鍚戦噺鐩稿悓锛堜緥濡傞兘绛変簬 (0,1)锛夛紝璇存槑鏂瑰悜鐩稿悓锛涘惁鍒欒鏄庤浆寮��
+// private boolean isTurning(AStarNavigateNode currNode, AStarNavigateNode nextNode) {
+// // 濡傛灉 currNode 娌℃湁鐖惰妭鐐癸紝璇存槑鏄捣鐐癸紝涓嶇畻杞集
+// if (currNode.getParent() == null) {
+// return false;
+// }
+// // 鍙栧嚭鍧愭爣
+// AStarNavigateNode parent = currNode.getParent();
+// int px = currNode.getX() - parent.getX(); // parent -> curr 鐨剎鍋忕Щ
+// int py = currNode.getY() - parent.getY(); // parent -> curr 鐨剏鍋忕Щ
+//
+// int nx = nextNode.getX() - currNode.getX(); // curr -> next 鐨剎鍋忕Щ
+// int ny = nextNode.getY() - currNode.getY(); // curr -> next 鐨剏鍋忕Щ
+//
+// // 濡傛灉 (px, py) 涓� (nx, ny) 涓嶄竴鏍凤紝灏辫鏄庤浆寮�
+// return (px != nx) || (py != ny);
+// }
+
}
--
Gitblit v1.9.1