From fe2d752b1f20cabead2620a3081ad77bc67526ae Mon Sep 17 00:00:00 2001
From: luxiaotao1123 <t1341870251@163.com>
Date: 星期二, 31 十二月 2024 16:09:44 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/AStarNavigateNode.java | 103 +++++++++++++++++++++++++
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java | 110 +++++++++++++++------------
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java | 11 +-
3 files changed, 170 insertions(+), 54 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 f3e6e74..7e4a8f2 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
@@ -6,6 +6,7 @@
import com.zy.acs.manager.common.utils.MapDataUtils;
import com.zy.acs.manager.core.constant.MapDataConstant;
import com.zy.acs.manager.core.service.astart.*;
+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.service.floyd.FloydNavigateService;
import com.zy.acs.manager.manager.entity.Code;
@@ -54,15 +55,15 @@
int[] startMapIdx = mapDataDispatcher.getCodeMatrixIdx(null, startCode.getData());
int[] endMapIdx = mapDataDispatcher.getCodeMatrixIdx(null, endCode.getData());
- NavigateNode startNode = new NavigateNode(startMapIdx[0], startMapIdx[1], startCode.getData());
- NavigateNode endNode = new NavigateNode(endMapIdx[0], endMapIdx[1], endCode.getData());
+ AStarNavigateNode startNode = new AStarNavigateNode(startMapIdx[0], startMapIdx[1], startCode.getData());
+ AStarNavigateNode endNode = new AStarNavigateNode(endMapIdx[0], endMapIdx[1], endCode.getData());
- NavigateNode finishNode = aStarNavigateService.execute(agvNo, startNode, endNode, lock, blackList, segment);
+ AStarNavigateNode finishNode = aStarNavigateService.execute(agvNo, startNode, endNode, lock, blackList, segment);
if (null == finishNode) {
return new ArrayList<>();
}
- ArrayList<NavigateNode> navigateNodes = new ArrayList<>();
+ ArrayList<AStarNavigateNode> navigateNodes = new ArrayList<>();
while (finishNode != null) {
navigateNodes.add(finishNode);
@@ -71,7 +72,7 @@
Collections.reverse(navigateNodes);
- List<String> navigatePath = navigateNodes.stream().map(NavigateNode::getCodeData).collect(Collectors.toList());
+ List<String> navigatePath = navigateNodes.stream().map(AStarNavigateNode::getCodeData).collect(Collectors.toList());
// max count of steps
if (navigatePath.size() > MapDataConstant.MAX_STEPS_SINGLE) {
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 656cd63..b8e0a90 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
@@ -1,12 +1,10 @@
package com.zy.acs.manager.core.service.astart;
-import com.zy.acs.common.constant.RedisConstant;
-import com.zy.acs.common.utils.GsonUtils;
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.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;
@@ -41,15 +39,15 @@
@Autowired
private ConfigService configService;
- public synchronized NavigateNode execute(String agvNo, NavigateNode start, NavigateNode end
+ 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<>();
- Set<NavigateNode> existNodes = new HashSet<>();
+ PriorityQueue<AStarNavigateNode> openQueue = new PriorityQueue<>();
+ Set<AStarNavigateNode> existNodes = new HashSet<>();
openQueue.add(start);
existNodes.add(start);
@@ -60,10 +58,14 @@
String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(null);
while (!openQueue.isEmpty()) {
// 鍙栦紭鍏堥槦鍒楅《閮ㄥ厓绱犲苟涓旀妸杩欎釜鍏冪礌浠嶰pen琛ㄤ腑鍒犻櫎锛屽彇F鍊兼渶灏忕殑鑺傜偣
- NavigateNode currentNode = openQueue.poll();
+ AStarNavigateNode currentNode = openQueue.poll();
- List<NavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
- for (NavigateNode node : neighbourNodes) {
+ // 缁堢偣
+ if (currentNode.getX() == end.getX() && currentNode.getY() == end.getY()) {
+ return currentNode;
+ }
+ List<AStarNavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
+ for (AStarNavigateNode node : neighbourNodes) {
node.setCodeData(codeMatrix[node.getX()][node.getY()]);
boolean isEndNode = node.getX() == end.getX() && node.getY() == end.getY();
@@ -140,23 +142,19 @@
}
}
- //鎵惧埌鐩爣缁撶偣灏辫繑鍥�
- if (isEndNode) {
- //骞朵笖璁＄畻鍑篏锛� F锛� H绛夊��
- node.initNode(currentNode, end);
- return node;
- }
-
- // G + H + T (瀵瑰惎鍙戝嚱鏁板鍔犲幓鎷愮偣鏂规calcNodeTurnCost)
- int gCost = calcNodeCost(currentNode, node);
-
if (OPEN_TURN_COST_WEIGHT) {
- gCost += calcNodeTurnCost(currentNode, node, end);
+ if (this.isTurning(currentNode, node)) {
+ weight += WEIGHT_CALC_FACTOR;
+ node.setTurnCount(currentNode.getTurnCount() + 1);
+ } else {
+ // 鏂瑰悜娌″彉
+ node.setTurnCount(currentNode.getTurnCount());
+ }
}
//杩涜璁＄畻瀵� 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());
@@ -165,22 +163,22 @@
existNodes.add(node);
}
}
- //濡傛灉閬嶅巻瀹屾墍鏈夊嚭鐜扮殑缁撶偣閮芥病鏈夋壘鍒版渶缁堢殑缁撶偣锛岃繑鍥瀗ull
+
return null;
}
// 鑾峰彇鍥涘懆鑺傜偣
- private List<NavigateNode> getNeighborNodes(NavigateNode currentNode, int[][] mapMatrix, Set<NavigateNode> existNodes) {
+ private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes) {
int x = currentNode.getX();
int y = currentNode.getY();
- List<NavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
+ List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
- 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
+ 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
);
possibleNodes.parallelStream()
@@ -191,15 +189,15 @@
return neighbourNodes;
}
- private NavigateNode extendNeighborNodes(NavigateNode currentNode, NavigateNode extendNode, int[][] mapMatrix, Set<NavigateNode> existNodes, Integer dx, Integer dy) {
- NavigateNode nextNode;
+ 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();
@@ -233,27 +231,41 @@
//------------------A*鍚彂鍑芥暟------------------//
//璁＄畻閫氳繃鐜板湪鐨勭粨鐐圭殑浣嶇疆鍜屾渶缁堢粨鐐圭殑浣嶇疆璁＄畻H鍊�(鏇煎搱椤挎硶锛氬潗鏍囧垎鍒彇宸�肩浉鍔�)
- private int calcNodeCost(NavigateNode node1, NavigateNode node2) {
+ 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 0;
+ // 杞集鍒ゆ柇锛氬彧鍏佽鈥滃瀭鐩存垨姘村钩鈥濊繍鍔�
+ private boolean isTurning(AStarNavigateNode currNode, AStarNavigateNode nextNode) {
+ // 绗竴涓偣
+ if (currNode.getParent() == null) {
+ return false;
}
-
- // 鎷愬悜缁堢偣鐨勭偣
- if (nextNode.getX() == endNode.getX() || nextNode.getY() == endNode.getY()) {
- return 1;
- }
-
- // 鏅�氭嫄鐐�
- return 1;
+ 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);
+// }
+
}
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/AStarNavigateNode.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/AStarNavigateNode.java
new file mode 100644
index 0000000..e203a45
--- /dev/null
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/AStarNavigateNode.java
@@ -0,0 +1,103 @@
+package com.zy.acs.manager.core.service.astart.domain;
+
+import lombok.Data;
+import lombok.extern.slf4j.Slf4j;
+
+import java.io.Serializable;
+import java.util.Objects;
+import java.util.Optional;
+
+/**
+ * A*瀵昏矾绠楁硶Node鑺傜偣
+ */
+@Slf4j
+@Data
+public class AStarNavigateNode implements Comparable<AStarNavigateNode>, Cloneable, Serializable {
+
+ private static final long serialVersionUID = 1L;
+
+ private int x; //鍧愭爣x
+ private int y; //鍧愭爣y
+ private int turnCount; //杞集娆℃暟
+
+ private int G; //宸茬粡鑺辫垂鐨勬鏁�
+ private int H; //灏嗚鑺辫垂鐨勬鏁�
+ private int F; //缁煎悎鑺辫垂鐨勬鏁�
+
+ private AStarNavigateNode parent; //鐖惰妭鐐�
+ private Integer lastDistance; // 璺濈涓婁釜鑺傜偣璺濈
+ private Integer weight; // G 鏉冮噸
+// private boolean turningPoint; //鏄惁涓烘嫄鐐�
+// private Integer moveDistance; // 鎬昏璧拌窛绂�
+// private String direction; //琛岃蛋鏂瑰悜
+
+ private String codeData;
+
+ public AStarNavigateNode(int x, int y) {
+ this.x = x;
+ this.y = y;
+ this.turnCount = 0;
+ }
+
+ public AStarNavigateNode(int x, int y, String codeData) {
+ this.x = x;
+ this.y = y;
+ this.turnCount = 0;
+ this.codeData = codeData;
+ }
+
+ //閫氳繃缁撶偣鐨勫潗鏍囧拰鐩爣缁撶偣鐨勫潗鏍囧彲浠ヨ绠楀嚭F锛� G锛� H涓変釜灞炴��
+ //闇�瑕佷紶鍏ヨ繖涓妭鐐圭殑涓婁竴涓妭鐐瑰拰鏈�缁堢殑缁撶偣
+ public void initNode(AStarNavigateNode father, AStarNavigateNode end) {
+ this.parent = father;
+ if (this.parent != null) {
+ //璧拌繃鐨勬鏁扮瓑浜庣埗鑺傜偣璧拌繃鐨勬鏁板姞涓�
+ this.G = this.parent.G
+ + Optional.ofNullable(this.lastDistance).orElse(0)
+ + Optional.ofNullable(this.weight).orElse(0);
+ } else { //鐖惰妭鐐逛负绌轰唬琛ㄥ畠鏄涓�涓粨鐐�
+ this.G = 0;
+ }
+
+ //浠ヤ笅璁＄畻鏂规涓虹畻娉曞師濮嬫柟妗堬紝娌℃湁鍘绘嫄鐐规柟妗堛�傚凡琚玈olution璁＄畻鏃惰嚜鍔ㄨ鐩栥��
+ //璁＄畻閫氳繃鐜板湪鐨勭粨鐐圭殑浣嶇疆鍜屾渶缁堢粨鐐圭殑浣嶇疆璁＄畻H鍊�(鏇煎搱椤挎硶锛氬潗鏍囧垎鍒彇宸�肩浉鍔�)
+ this.H = Math.abs(this.x - end.x) + Math.abs(this.y - end.y);
+ this.F = this.G + this.H;
+ }
+
+ @Override
+ public int compareTo(AStarNavigateNode o) {
+ return Integer.compare(this.F, o.F);
+ }
+
+ @Override
+ public AStarNavigateNode clone() {
+ try {
+ return (AStarNavigateNode) super.clone();
+ } catch (CloneNotSupportedException e) {
+ log.error(this.getClass().getSimpleName(), e);
+ }
+ return null;
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) return true;
+ if (!(obj instanceof AStarNavigateNode)) return false;
+ AStarNavigateNode that = (AStarNavigateNode) obj;
+ return this.x == that.x
+ && this.y == that.y
+// && this.turnCount == that.turnCount // ** 杞集浼樺寲锛屼弗閲嶅奖鍝嶆�ц兘
+ ;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(
+ this.x
+ , this.y
+// , this.turnCount // ** 杞集浼樺寲锛屼弗閲嶅奖鍝嶆�ц兘
+ );
+ }
+
+}
--
Gitblit v1.9.1