From 93d8a38f9fd0746b9ce6ac7541bf2b8b48f7c63c Mon Sep 17 00:00:00 2001
From: luxiaotao1123 <t1341870251@163.com>
Date: 星期三, 18 十二月 2024 13:11:24 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java | 159 ++++++++++++++++++++--------------------------------
1 files changed, 61 insertions(+), 98 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 2e41211..1f506ac 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
@@ -7,16 +7,15 @@
import com.zy.acs.manager.core.service.LaneService;
import com.zy.acs.manager.core.service.astart.domain.DynamicNode;
import com.zy.acs.manager.manager.entity.Route;
-import com.zy.acs.manager.manager.service.AgvService;
+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;
-import java.util.ArrayList;
-import java.util.List;
-import java.util.PriorityQueue;
+import java.util.*;
/**
* Created by vincent on 6/12/2024
@@ -26,7 +25,9 @@
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;
@@ -35,21 +36,21 @@
@Autowired
private MapDataDispatcher mapDataDispatcher;
@Autowired
- private AgvService agvService;
+ private JamService jamService;
@Autowired
private LaneService laneService;
@Autowired
private ConfigService configService;
public synchronized NavigateNode execute(String agvNo, NavigateNode start, NavigateNode end
- , Boolean lock, List<String> blackList) {
+ , 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<>();
+ Set<NavigateNode> existNodes = new HashSet<>();
openQueue.add(start);
existNodes.add(start);
@@ -58,32 +59,27 @@
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(null);
String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(null);
-// List<String> included = new ArrayList<>();
-// if (!Cools.isEmpty(whiteList)) {
-// included.addAll(whiteList);
-// }
-// included.add(agvNo);
-// List<VehicleDto> vehicleDtoList = agvService.getVehicleDtoList(included);
-
- while (openQueue.size() > 0) {
+ while (!openQueue.isEmpty()) {
// 鍙栦紭鍏堥槦鍒楅《閮ㄥ厓绱犲苟涓旀妸杩欎釜鍏冪礌浠嶰pen琛ㄤ腑鍒犻櫎锛屽彇F鍊兼渶灏忕殑鑺傜偣
NavigateNode currentNode = openQueue.poll();
- // 瀵瑰綋鍓嶇粨鐐硅繘琛屾墿灞曪紝寰楀埌涓�涓洓鍛ㄧ粨鐐圭殑鏁扮粍
- ArrayList<NavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
- // 瀵硅繖涓粨鐐归亶鍘嗭紝鐪嬫槸鍚︽湁鐩爣缁撶偣鍑虹幇
- label: for (NavigateNode node : neighbourNodes) {
- // 鑺傜偣瀛樺湪鍏朵粬杞﹁締
-// for (VehicleDto vehicleDto : vehicleDtoList) {
-// if (node.getCodeData().equals(vehicleDto.getPosCode())) {
-// if (!Cools.isEmpty(blackList) && blackList.contains(vehicleDto.getVehicle())) {
-// continue label;
-// }
-// if (lock) {
-// continue label;
-// }
-// }
-// }
+ ArrayList<NavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
+ for (NavigateNode node : neighbourNodes) {
+ 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;
+ }
+ }
// 鑺傜偣琚崰鐢�
DynamicNode dynamicNode = dynamicMatrix[node.getX()][node.getY()];
@@ -92,9 +88,16 @@
if (!vehicle.equals(DynamicNodeType.ACCESS.val)) {
if (!vehicle.equals(agvNo)) {
- if (!Cools.isEmpty(blackList) && blackList.contains(vehicle)) {
- 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);
+ }
}
+
if (lock) {
continue;
}
@@ -109,9 +112,6 @@
List<String> otherWaveList = MapDataUtils.hasOtherWave(waveNodeList, agvNo);
if (!Cools.isEmpty(otherWaveList)) {
- if (!Cools.isEmpty(blackList) && 0 < Cools.getIntersection(otherWaveList, blackList).size()) {
- continue;
- }
if (lock) {
continue;
@@ -122,7 +122,7 @@
// 鍗曞贩閬撹溅杈嗗杞芥暟閲�
Lane lane = laneService.search(node.getCodeData());
if (null != lane) {
- int otherVehicleCount = 0;
+ Set<String> lanVehicleSet = new HashSet<>();
List<String> laneCodes = lane.getCodes();
for (String laneCodeData : laneCodes) {
@@ -133,31 +133,34 @@
assert !laneVehicle.equals(DynamicNodeType.BLOCK.val);
if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) {
if (!laneVehicle.equals(agvNo)) {
- otherVehicleCount++;
+ lanVehicleSet.add(laneVehicle);
// redis.setObject(RedisConstant.AGV_TO_STANDBY_FLAG, laneVehicle, true, 30);
}
}
}
- if (otherVehicleCount + 1 > maxAgvCountInLane) {
- if (lock) {
- continue;
- }
+ if (lanVehicleSet.size() + 1 > maxAgvCountInLane) {
+ continue;
}
}
//鎵惧埌鐩爣缁撶偣灏辫繑鍥�
- if (node.getX() == end.getX() && node.getY() == end.getY()) {
+ if (isEndNode) {
//骞朵笖璁＄畻鍑篏锛� F锛� H绛夊��
node.initNode(currentNode, end);
return node;
}
// G + H + T (瀵瑰惎鍙戝嚱鏁板鍔犲幓鎷愮偣鏂规calcNodeTurnCost)
- int gCost = calcNodeCost(currentNode, node) * (OPEN_TURN_COST_WEIGHT ? calcNodeTurnCost(currentNode, node, end) : 1);
+ int gCost = calcNodeCost(currentNode, node);
+
+ if (OPEN_TURN_COST_WEIGHT) {
+ gCost += calcNodeTurnCost(currentNode, node, end);
+ }
//杩涜璁＄畻瀵� G, F, H 绛夊��
+ node.setWeight(weight);
node.setLastDistance(gCost);
node.initNode(currentNode, end);
node.setH(calcNodeCost(node, end));
@@ -172,7 +175,7 @@
}
// 鑾峰彇鍥涘懆鑺傜偣
- private ArrayList<NavigateNode> getNeighborNodes(NavigateNode currentNode, int[][] mapMatrix, List<NavigateNode> existNodes) {
+ private ArrayList<NavigateNode> getNeighborNodes(NavigateNode currentNode, int[][] mapMatrix, Set<NavigateNode> existNodes) {
//鑾峰彇褰撳墠缁撶偣鐨剎, y
int x = currentNode.getX();
int y = currentNode.getY();
@@ -180,29 +183,29 @@
ArrayList<NavigateNode> neighbourNodes = new ArrayList<>();
NavigateNode rightNode = extendNeighborNodes(currentNode, new NavigateNode(x, y + 1), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, rightNode, mapMatrix, existNodes)) {
+ if (is_valid(currentNode, rightNode)) {
neighbourNodes.add(rightNode);
}
NavigateNode leftNode = extendNeighborNodes(currentNode, new NavigateNode(x, y - 1), mapMatrix, existNodes, null, null);
- if (is_valid(currentNode, leftNode, mapMatrix, existNodes)) {
+ 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, mapMatrix, existNodes)) {
+ 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, mapMatrix, existNodes)) {
+ if (is_valid(currentNode, bottomNode)) {
neighbourNodes.add(bottomNode);
}
return neighbourNodes;
}
- private NavigateNode extendNeighborNodes(NavigateNode currentNode, NavigateNode extendNode, int[][] mapMatrix, List<NavigateNode> existNodes, Integer dx, Integer dy) {
+ private NavigateNode extendNeighborNodes(NavigateNode currentNode, NavigateNode extendNode, int[][] mapMatrix, Set<NavigateNode> existNodes, Integer dx, Integer dy) {
NavigateNode nextNode = null;
if (null == dx || null == dy) {
@@ -227,7 +230,7 @@
} else {
- if (isExist(nextNode, existNodes)) {
+ if (existNodes.contains(nextNode)) {
return null;
}
@@ -254,49 +257,12 @@
}
}
- private boolean is_valid(NavigateNode currentNode, NavigateNode node, int[][] mapMatrix, List<NavigateNode> existNodes) {
+ private boolean is_valid(NavigateNode currentNode, NavigateNode node) {
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;
-// }
-
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) {
@@ -318,7 +284,9 @@
//璁＄畻閫氳繃鐜板湪鐨勭粨鐐圭殑浣嶇疆鍜屾渶缁堢粨鐐圭殑浣嶇疆璁＄畻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());
}
@@ -329,21 +297,16 @@
|| nextNode.getX() == currNode.getParent().getX()
|| nextNode.getY() == currNode.getParent().getY()
) {
- return 1;
+ return 0;
}
// 鎷愬悜缁堢偣鐨勭偣
if (nextNode.getX() == endNode.getX() || nextNode.getY() == endNode.getY()) {
- return 2;
+ return 1;
}
// 鏅�氭嫄鐐�
- /*
- 鎷愮偣鍒ゆ柇閫昏緫
- 鎷垮埌鐖惰妭鐐瑰拰涓嬩竴鑺傜偣
- 閫氳繃鍒ゆ柇鐖惰妭鐐瑰拰涓嬩竴鑺傜偣鐨剎鏁版嵁鍜寉鏁版嵁閮戒笉鐩稿悓鏃讹紝鍒欒〃鏄庡綋鍓嶅潗鏍囨槸涓�涓嫄鐐�
- */
- return 3;
+ return 1;
}
}
--
Gitblit v1.9.1