From a6e17e938d1a5f18c758f996ad7b782a9256ddda Mon Sep 17 00:00:00 2001
From: luxiaotao1123 <t1341870251@163.com>
Date: 星期一, 23 十二月 2024 10:50:44 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java | 98 +++++++++++-------------------------------------
1 files changed, 23 insertions(+), 75 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 d3fc394..f26ba52 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
@@ -8,11 +8,8 @@
import com.zy.acs.manager.core.service.LaneService;
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.Route;
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 org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;
@@ -31,10 +28,6 @@
public static final int WEIGHT_CALC_FACTOR = 1;
- @Autowired
- private CodeService codeService;
- @Autowired
- private RouteService routeService;
@Autowired
private MapDataDispatcher mapDataDispatcher;
@Autowired
@@ -58,15 +51,16 @@
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.isEmpty()) {
// 鍙栦紭鍏堥槦鍒楅《閮ㄥ厓绱犲苟涓旀妸杩欎釜鍏冪礌浠嶰pen琛ㄤ腑鍒犻櫎锛屽彇F鍊兼渶灏忕殑鑺傜偣
NavigateNode currentNode = openQueue.poll();
ArrayList<NavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
for (NavigateNode node : neighbourNodes) {
+ node.setCodeData(codeMatrix[node.getX()][node.getY()]);
boolean isEndNode = node.getX() == end.getX() && node.getY() == end.getY();
int weight = 0;
@@ -178,37 +172,28 @@
// 鑾峰彇鍥涘懆鑺傜偣
private ArrayList<NavigateNode> getNeighborNodes(NavigateNode currentNode, int[][] mapMatrix, Set<NavigateNode> existNodes) {
- //鑾峰彇褰撳墠缁撶偣鐨剎, y
int x = currentNode.getX();
int y = currentNode.getY();
- //濡傛灉褰撳墠缁撶偣鐨勯偦缁撶偣鍚堟硶锛屽氨鍔犲叆鍒皀eighbour_node
+
ArrayList<NavigateNode> neighbourNodes = new ArrayList<>();
- NavigateNode rightNode = extendNeighborNodes(currentNode, new NavigateNode(x, y + 1), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, rightNode)) {
- neighbourNodes.add(rightNode);
- }
+ List<NavigateNode> possibleNodes = Arrays.asList(
+ new NavigateNode(x, y + 1), // right
+ new NavigateNode(x, y - 1), // left
+ new NavigateNode(x - 1, y), // up
+ new NavigateNode(x + 1, y) // down
+ );
- NavigateNode leftNode = extendNeighborNodes(currentNode, new NavigateNode(x, y - 1), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, leftNode)) {
- neighbourNodes.add(leftNode);
- }
-
- NavigateNode topNode = extendNeighborNodes(currentNode, new NavigateNode(x - 1, y), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, topNode)) {
- neighbourNodes.add(topNode);
- }
-
- NavigateNode bottomNode = extendNeighborNodes(currentNode, new NavigateNode(x + 1, y), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, bottomNode)) {
- neighbourNodes.add(bottomNode);
- }
+ possibleNodes.parallelStream()
+ .map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
+ .filter(Objects::nonNull)
+ .forEach(neighbourNodes::add);
return neighbourNodes;
}
private NavigateNode extendNeighborNodes(NavigateNode currentNode, NavigateNode extendNode, int[][] mapMatrix, Set<NavigateNode> existNodes, Integer dx, Integer dy) {
- NavigateNode nextNode = null;
+ NavigateNode nextNode;
if (null == dx || null == dy) {
dx = extendNode.getX() - currentNode.getX();
@@ -229,64 +214,27 @@
if (mapMatrix[x][y] == MapNodeType.DISABLE.val) {
return extendNeighborNodes(currentNode, nextNode, mapMatrix, existNodes, dx, dy);
-
- } else {
-
- if (existNodes.contains(nextNode)) {
- 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);
-
- // 鍒ゆ柇閫氳繃鎬�
- String routeKey = RouteGenerator.generateRouteKey(currentNodeCodeData, nextNodeCodeData);
- Object routeVal = redis.getMap(RedisConstant.AGV_MAP_ROUTE_HASH_FLAG, routeKey);
- if (routeVal == null || !(Boolean) routeVal) {
- return null;
- }
-
- return nextNode;
}
- }
- private boolean is_valid(NavigateNode currentNode, NavigateNode node) {
- if (null == node) {
- return false;
- }
- return true;
- }
+ assert mapMatrix[x][y] == MapNodeType.ENABLE.val;
- private boolean isExist(NavigateNode node, List<NavigateNode> existNodes) {
- for (NavigateNode existNode : existNodes) {
- if (this.isSame(node, existNode)) {
- return true;
- }
+ if (existNodes.contains(nextNode)) {
+ return null;
}
- return false;
- }
- private boolean isSame(NavigateNode o1, NavigateNode o2) {
- if (Cools.isEmpty(o1, o2)) {
- return false;
+ // 鍒ゆ柇閫氳繃鎬�
+ String routeCdaKey = RouteGenerator.generateRouteCdaKey(new int[]{currentNode.getX(), currentNode.getY()}, new int[]{nextNode.getX(), nextNode.getY()});
+ if (!mapDataDispatcher.validRouteCdaKey(routeCdaKey)) {
+ return null;
}
- 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()));
return Math.abs(node2.getX() - node1.getX()) + Math.abs(node2.getY() - node1.getY());
}
--
Gitblit v1.9.1