From 96f987b030f4c961a985f07079ba12abd865fdb2 Mon Sep 17 00:00:00 2001
From: Junjie <DELL@qq.com>
Date: 星期三, 17 十二月 2025 08:12:24 +0800
Subject: [PATCH] #
---
src/main/java/com/zy/common/utils/NavigateSolution.java | 442 +++++++++++++++++++++++++++++++++----------------------
1 files changed, 264 insertions(+), 178 deletions(-)
diff --git a/src/main/java/com/zy/common/utils/NavigateSolution.java b/src/main/java/com/zy/common/utils/NavigateSolution.java
index 681c54d..e189e68 100644
--- a/src/main/java/com/zy/common/utils/NavigateSolution.java
+++ b/src/main/java/com/zy/common/utils/NavigateSolution.java
@@ -1,44 +1,22 @@
package com.zy.common.utils;
import com.alibaba.fastjson.JSON;
+import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.mapper.EntityWrapper;
import com.core.common.SpringUtils;
import com.core.exception.CoolException;
+import com.zy.asrs.entity.BasMap;
+import com.zy.asrs.service.BasMapService;
import com.zy.common.model.NavigateNode;
-import com.zy.common.model.enums.NavigationMapType;
import com.zy.core.enums.MapNodeType;
-import com.zy.core.model.PythonResult;
-import com.zy.system.entity.Config;
-import com.zy.system.service.ConfigService;
-
-import java.io.BufferedReader;
-import java.io.IOException;
-import java.io.InputStreamReader;
import java.util.*;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.function.BiFunction;
/**
- * 鍥涘悜搴撴牳蹇�
* A*绠楁硶瀹炵幇绫�
*/
public class NavigateSolution {
-
- // -1 -> 澧欏锛� 0 -> 璐т綅锛� 1 -> 璧风偣 2 -> 缁堢偣 3-> 姣嶈建 4->杈撻�佺珯鐐� 5->鍏呯數妗� 6->灏忚溅鍙蛋杈撻�佺珯鐐� 66->灏忚溅 67->鎻愬崌鏈�
-
- int[][] map = {{}};
-
- public NavigateSolution(List<NavigationMapType> mapTypes, Integer lev, List<int[]> whitePoints, List<int[]> shuttlePoints) {
- //杞藉叆鍦板浘鎸囧畾灞傞珮鍦板浘
- NavigateMapData mapData = SpringUtils.getBean(NavigateMapData.class);
- int[][] data = mapData.getDataFromRedis(lev, mapTypes, whitePoints, shuttlePoints);
- if (data == null) {
- throw new CoolException("鍦板浘鏈浇鍏ワ紒");
- }
- this.map = data;
- }
-
- public NavigateSolution(int[][] data) {
- this.map = data;
- }
// Open琛ㄧ敤浼樺厛闃熷垪
public PriorityQueue<NavigateNode> Open = new PriorityQueue<NavigateNode>();
@@ -47,75 +25,96 @@
//鐢ㄦ潵瀛樻斁宸茬粡鍑虹幇杩囩殑缁撶偣銆�
Map<String, Integer> bestGMap = new HashMap<>();
- public String astarSearchPython(NavigateNode start, NavigateNode end, String pythonScriptPath) {
- String maps = JSON.toJSONString(map);
- String startStr = start.getX() + "-" + start.getY();
- String targetStr = end.getX() + "-" + end.getY();
-
- //榛樿鍦板浘姣嶈建鏂瑰悜x
- String mapDirection = "x";
- ConfigService configService = SpringUtils.getBean(ConfigService.class);
- if (configService != null) {
- Config config = configService.selectOne(new EntityWrapper<Config>()
- .eq("code", "direction_map")
- .eq("status", 1));
- if (config != null) {
- mapDirection = config.getValue();
- }
+ public List<List<NavigateNode>> getStationMap(int lev) {
+ BasMapService basMapService = SpringUtils.getBean(BasMapService.class);
+ BasMap basMap = basMapService.selectOne(new EntityWrapper<BasMap>().eq("lev", lev));
+ if (basMap == null) {
+ throw new CoolException("鍦板浘涓嶅瓨鍦�");
}
- ProcessBuilder processBuilder = new ProcessBuilder("python", pythonScriptPath, maps, startStr, targetStr, mapDirection);
- processBuilder.redirectErrorStream(true);
+ List<List<JSONObject>> dataList = JSON.parseObject(basMap.getData(), List.class);
- try {
- Process process = processBuilder.start();
+ List<List<NavigateNode>> navigateNodeList = new ArrayList<>();
+ for (int i = 0; i < dataList.size(); i++) {
+ List<JSONObject> row = dataList.get(i);
+ List<NavigateNode> navigateNodeRow = new ArrayList<>();
+ for (int j = 0; j < row.size(); j++) {
+ JSONObject map = row.get(j);
+ NavigateNode navigateNode = new NavigateNode(i, j);
- // 璇诲彇Python鑴氭湰鐨勮緭鍑�
- BufferedReader reader = new BufferedReader(new InputStreamReader(process.getInputStream()));
- String line;
- StringBuilder builder = new StringBuilder();
- while ((line = reader.readLine()) != null) {
- builder.append(line);
+ String nodeType = map.getString("type");
+ if(nodeType == null) {
+ navigateNode.setValue(MapNodeType.DISABLE.id);
+ }else if(nodeType.equals("devp") || nodeType.equals("merge")){
+ navigateNode.setValue(MapNodeType.NORMAL_PATH.id);
+
+ JSONObject valueObj = JSON.parseObject(map.getString("value"));
+ List<String> directionList = valueObj.getJSONArray("direction").toJavaList(String.class);
+ navigateNode.setDirectionList(directionList);
+ }else {
+ navigateNode.setValue(MapNodeType.DISABLE.id);
+ }
+
+ navigateNode.setNodeType(nodeType);
+ navigateNode.setNodeValue(map.getString("value"));
+ navigateNodeRow.add(navigateNode);
}
-
- // 绛夊緟Python鑴氭湰鎵ц瀹屾垚
- int exitCode = process.waitFor();
- if (exitCode != 0) {
- System.out.println("Python script exited with error code: " + exitCode);
- return null;
- }
- reader.close();
-
- if (builder.length() <= 0) {
- return null;
- }
-
- PythonResult result = JSON.parseObject(builder.toString(), PythonResult.class);
- if (result.getCalcResult() != 200) {
- return null;
- }
-
- String path = result.getPath();
- return path;
- } catch (IOException | InterruptedException e) {
- e.printStackTrace();
+ navigateNodeList.add(navigateNodeRow);
}
- return null;
+
+ return navigateNodeList;
}
- public NavigateNode astarSearchJava(NavigateNode start, NavigateNode end) {
- //榛樿鍦板浘姣嶈建鏂瑰悜x
- String mapDirection = "x";
- ConfigService configService = SpringUtils.getBean(ConfigService.class);
- if (configService != null) {
- Config config = configService.selectOne(new EntityWrapper<Config>()
- .eq("code", "direction_map")
- .eq("status", 1));
- if (config != null) {
- mapDirection = config.getValue();
- }
+ public List<List<NavigateNode>> getRgvTrackMap(int lev) {
+ BasMapService basMapService = SpringUtils.getBean(BasMapService.class);
+ BasMap basMap = basMapService.selectOne(new EntityWrapper<BasMap>().eq("lev", lev));
+ if (basMap == null) {
+ throw new CoolException("鍦板浘涓嶅瓨鍦�");
}
+ List<List<JSONObject>> dataList = JSON.parseObject(basMap.getData(), List.class);
+
+ List<List<NavigateNode>> navigateNodeList = new ArrayList<>();
+ for (int i = 0; i < dataList.size(); i++) {
+ List<JSONObject> row = dataList.get(i);
+ List<NavigateNode> navigateNodeRow = new ArrayList<>();
+ for (int j = 0; j < row.size(); j++) {
+ JSONObject map = row.get(j);
+ NavigateNode navigateNode = new NavigateNode(i, j);
+
+ String nodeType = map.getString("type");
+ if(nodeType == null) {
+ navigateNode.setValue(MapNodeType.DISABLE.id);
+ }else if(nodeType.equals("rgv")){
+ navigateNode.setValue(MapNodeType.NORMAL_PATH.id);
+ JSONObject valueObj = JSON.parseObject(map.getString("value"));
+
+ //RGV鏆備笉鎺у埗琛岃蛋鏂瑰悜锛岄粯璁や笂涓嬪乏鍙抽兘鍙蛋
+ List<String> directionList = new ArrayList<>();
+ directionList.add("top");
+ directionList.add("bottom");
+ directionList.add("left");
+ directionList.add("right");
+ navigateNode.setDirectionList(directionList);
+ }else {
+ navigateNode.setValue(MapNodeType.DISABLE.id);
+ }
+
+ navigateNode.setNodeType(nodeType);
+ navigateNode.setNodeValue(map.getString("value"));
+ navigateNodeRow.add(navigateNode);
+ }
+ navigateNodeList.add(navigateNodeRow);
+ }
+
+ return navigateNodeList;
+ }
+
+ public NavigateNode astarSearchJava(List<List<NavigateNode>> map, NavigateNode start, NavigateNode end) {
+ // 娓呯悊涓婃鎼滅储鐨勭姸鎬侊紝闃叉澶嶇敤瀹炰緥瀵艰嚧璁板綍娈嬬暀
+ this.Open.clear();
+ this.Close.clear();
+ this.bestGMap.clear();
//鎶婄涓�涓紑濮嬬殑缁撶偣鍔犲叆鍒癘pen琛ㄤ腑
this.Open.add(start);
//涓诲惊鐜�
@@ -130,9 +129,13 @@
//灏嗚繖涓粨鐐瑰姞鍏ュ埌Close琛ㄤ腑
Close.add(current_node);
//瀵瑰綋鍓嶇粨鐐硅繘琛屾墿灞曪紝寰楀埌涓�涓洓鍛ㄧ粨鐐圭殑鏁扮粍
- ArrayList<NavigateNode> neighbour_node = extend_current_node(current_node, mapDirection);
+ ArrayList<NavigateNode> neighbour_node = extend_current_node(map, current_node);
//瀵硅繖涓粨鐐归亶鍘嗭紝鐪嬫槸鍚︽湁鐩爣缁撶偣鍑虹幇
for (NavigateNode node : neighbour_node) {
+ // 宸插湪鍏抽棴鍒楄〃涓殑鑺傜偣涓嶅啀澶勭悊锛岄伩鍏嶇埗閾惧弽澶嶆敼鍐欏舰鎴愮幆
+ if (Close.contains(node)) {
+ continue;
+ }
// G + H + E (瀵瑰惎鍙戝嚱鏁板鍔犲幓鎷愮偣鏂规calcNodeExtraCost)
int gCost = calcNodeExtraCost(current_node, node, end);
@@ -144,7 +147,8 @@
String key = node.getX() + "_" + node.getY();
Integer recordedG = bestGMap.get(key);
- if (recordedG == null || node.getG() <= recordedG) {
+ // 浠呭綋鎵惧埌鏇村皬鐨勪唬浠锋椂鎵嶆洿鏂颁笌鍏pen锛岄伩鍏嶇瓑浠蜂唬浠峰弽澶嶅叆闃熷鑷寸埗閾炬姈鍔�
+ if (recordedG == null || node.getG() < recordedG) {
bestGMap.put(key, node.getG());
Open.add(node);
@@ -155,94 +159,150 @@
return null;
}
+ public List<List<NavigateNode>> allSimplePaths(
+ List<List<NavigateNode>> map,
+ NavigateNode start,
+ NavigateNode end,
+ int maxDepth, // 鏈�澶ф鏁�(杈规暟). 寤鸿锛�50/100/200锛�<=0 琛ㄧず涓嶉檺鍒讹紙涓嶅缓璁級
+ int maxPaths, // 鏈�澶ц繑鍥炴潯鏁�. 寤鸿锛�100/500/2000锛�<=0 琛ㄧず涓嶉檺鍒讹紙涓嶅缓璁級
+ int maxCost // 鏈�澶ф�讳唬浠�(鍚嫄鐐规儵缃�). <=0 琛ㄧず涓嶉檺鍒�
+ ) {
+ List<List<NavigateNode>> results = new ArrayList<>();
+ if (map == null || map.isEmpty() || map.get(0).isEmpty()) return results;
+ if (start == null || end == null) return results;
+ if (start.getValue() == MapNodeType.DISABLE.id || end.getValue() == MapNodeType.DISABLE.id) return results;
- public ArrayList<NavigateNode> extend_current_node(NavigateNode current_node, String mapDirection) {
+ // visited 鐢ㄥ潗鏍� key锛岄伩鍏� NavigateNode equals/hashCode 涓嶅彲闈犲鑷撮噸澶嶅垽鏂け鏁�
+ Set<String> visited = new HashSet<>(map.size() * map.get(0).size() * 2);
+ LinkedList<NavigateNode> path = new LinkedList<>();
+
+ String startKey = keyOf(start);
+ visited.add(startKey);
+ path.add(start);
+
+ AtomicInteger pathCount = new AtomicInteger(0);
+
+ dfsAllSimplePaths(map, start, end,
+ visited, path, results,
+ 0, // depth
+ 0, // cost
+ maxDepth, maxPaths, maxCost,
+ pathCount
+ );
+
+ return results;
+ }
+
+ /**
+ * DFS + 鍥炴函锛氭灇涓炬墍鏈夌畝鍗曡矾寰勶紙璺緞涓笉鍏佽閲嶅鑺傜偣锛�
+ */
+ private void dfsAllSimplePaths(
+ List<List<NavigateNode>> map,
+ NavigateNode current,
+ NavigateNode end,
+ Set<String> visited,
+ LinkedList<NavigateNode> path,
+ List<List<NavigateNode>> results,
+ int depth, // 褰撳墠姝ユ暟锛堣竟鏁帮級
+ int cost, // 褰撳墠鎬讳唬浠凤紙浣犲彲浠ヨ涓烘槸 g锛�
+ int maxDepth,
+ int maxPaths,
+ int maxCost,
+ AtomicInteger pathCount
+ ) {
+ // 闃茬垎锛氭潯鏁伴檺鍒�
+ if (maxPaths > 0 && pathCount.get() >= maxPaths) return;
+
+ // 鍒拌揪缁堢偣锛氭敹闆嗚矾寰�
+ if (current.getX() == end.getX() && current.getY() == end.getY()) {
+ results.add(new ArrayList<>(path));
+ pathCount.incrementAndGet();
+ return;
+ }
+
+ // 闃茬垎锛氭繁搴﹂檺鍒讹紙depth 琛ㄧず宸茶蛋鐨勮竟鏁帮級
+ if (maxDepth > 0 && depth >= maxDepth) return;
+
+ // 鎵╁睍閭诲眳锛堜弗鏍煎鐢ㄤ綘鑷繁鐨勫彲琛岃蛋鏂瑰悜瑙勫垯锛�
+ ArrayList<NavigateNode> neighbors = extend_current_node(map, current);
+ if (neighbors == null || neighbors.isEmpty()) return;
+
+ for (NavigateNode next : neighbors) {
+ // 闃茬垎锛氭潯鏁伴檺鍒�
+ if (maxPaths > 0 && pathCount.get() >= maxPaths) return;
+
+ if (next == null) continue;
+ if (next.getValue() == MapNodeType.DISABLE.id) continue;
+
+ String nk = keyOf(next);
+
+ // 绠�鍗曡矾寰勶細涓嶅厑璁搁噸澶嶈妭鐐�
+ if (visited.contains(nk)) continue;
+
+ // 浣犵殑浠ｄ环瑙勫垯锛氭瘡姝� 1 + 鎷愮偣鎯╃綒
+ int stepCost = 1 + calcNodeExtraCost(current, next, end);
+ int newCost = cost + stepCost;
+
+ // 闃茬垎锛氭�讳唬浠烽檺鍒�
+ if (maxCost > 0 && newCost > maxCost) continue;
+
+ // 杩涘叆
+ visited.add(nk);
+ path.addLast(next);
+
+ dfsAllSimplePaths(map, next, end,
+ visited, path, results,
+ depth + 1,
+ newCost,
+ maxDepth, maxPaths, maxCost,
+ pathCount
+ );
+
+ // 鍥炴函
+ path.removeLast();
+ visited.remove(nk);
+ }
+ }
+
+ private String keyOf(NavigateNode n) {
+ return n.getX() + "_" + n.getY();
+ }
+
+ public ArrayList<NavigateNode> extend_current_node(List<List<NavigateNode>> map, NavigateNode current_node) {
//鑾峰彇褰撳墠缁撶偣鐨剎, y
int x = current_node.getX();
int y = current_node.getY();
//濡傛灉褰撳墠缁撶偣鐨勯偦缁撶偣鍚堟硶锛屽氨鍔犲叆鍒皀eighbour_node
ArrayList<NavigateNode> neighbour_node = new ArrayList<NavigateNode>();
-// if (map[x][y] == 0 || map[x][y] == 3) {
-// //鍙湁瀛愯建鍜屾瘝杞ㄦ墠鑳借繘琛屽乏鍙崇Щ鍔�
-// if (is_valid(x, y + 1))
-// {
-// Node node = new Node(x, y + 1);
-// neighbour_node.add(node);
-// }
-// if (is_valid(x, y - 1))
-// {
-// Node node = new Node(x, y - 1);
-// neighbour_node.add(node);
-// }
-// }
-//
-// if (map[x][y] == 3) {
-// //鍙湁姣嶈建鎵嶈兘杩涜涓婁笅绉诲姩
-// if (is_valid(x + 1, y))
-// {
-// Node node = new Node(x + 1, y);
-// neighbour_node.add(node);
-// }
-// if (is_valid(x - 1, y))
-// {
-// Node node = new Node(x -1, y);
-// neighbour_node.add(node);
-// }
-// }
- if (mapDirection.equals("x")) {//姣嶈建x鏂瑰悜
- if (map[x][y] == MapNodeType.MAIN_PATH.id) {
- //姣嶈建鎵嶈兘杩涜涓婁笅绉诲姩
- if (is_valid(x + 1, y)) {
- NavigateNode node = new NavigateNode(x + 1, y);
- node.setNodeValue(map[x + 1][y]);
- neighbour_node.add(node);
- }
- if (is_valid(x - 1, y)) {
- NavigateNode node = new NavigateNode(x - 1, y);
- node.setNodeValue(map[x - 1][y]);
- neighbour_node.add(node);
- }
- }
+ if(current_node.getValue() == MapNodeType.DISABLE.id) {
+ return neighbour_node;
+ }
- if (map[x][y] == MapNodeType.NORMAL_PATH.id || map[x][y] == MapNodeType.MAIN_PATH.id || map[x][y] == MapNodeType.CONVEYOR_CAR_GO.id || map[x][y] == MapNodeType.CONVEYOR.id || map[x][y] == MapNodeType.CHARGE.id || map[x][y] == MapNodeType.LIFT.id) {
- //瀛愯建鍜屾瘝杞ㄣ�佸皬杞﹀彲璧拌緭閫佺嚎銆佸厖鐢垫々銆佹彁鍗囨満鎵嶈兘杩涜宸﹀彸绉诲姩
- if (is_valid(x, y + 1)) {
- NavigateNode node = new NavigateNode(x, y + 1);
- node.setNodeValue(map[x][y + 1]);
- neighbour_node.add(node);
- }
- if (is_valid(x, y - 1)) {
- NavigateNode node = new NavigateNode(x, y - 1);
- node.setNodeValue(map[x][y - 1]);
- neighbour_node.add(node);
- }
- }
- } else if (mapDirection.equals("y")) {//姣嶈建y鏂瑰悜
- if (map[x][y] == MapNodeType.MAIN_PATH.id) {
- //姣嶈建鎵嶈兘杩涜宸﹀彸绉诲姩
- if (is_valid(x, y + 1)) {
- NavigateNode node = new NavigateNode(x, y + 1);
- node.setNodeValue(map[x][y + 1]);
- neighbour_node.add(node);
- }
- if (is_valid(x, y - 1)) {
- NavigateNode node = new NavigateNode(x, y - 1);
- node.setNodeValue(map[x][y - 1]);
- neighbour_node.add(node);
- }
- }
+ List<String> directionList = current_node.getDirectionList();
+ if(directionList == null || directionList.size() == 0) {
+ return neighbour_node;
+ }
- if (map[x][y] == MapNodeType.NORMAL_PATH.id || map[x][y] == MapNodeType.MAIN_PATH.id || map[x][y] == MapNodeType.CONVEYOR_CAR_GO.id || map[x][y] == MapNodeType.CONVEYOR.id || map[x][y] == MapNodeType.CHARGE.id || map[x][y] == MapNodeType.LIFT.id) {
- //瀛愯建鍜屾瘝杞ㄣ�佸皬杞﹀彲璧拌緭閫佺嚎銆佸厖鐢垫々銆佹彁鍗囨満鎵嶈兘杩涜涓婁笅绉诲姩
- if (is_valid(x + 1, y)) {
- NavigateNode node = new NavigateNode(x + 1, y);
- node.setNodeValue(map[x + 1][y]);
+ for(String direction : directionList) {
+ if(direction.equals("top")) {
+ NavigateNode node = getValidNavigateNode(map, x - 1, y);
+ if(node != null) {
neighbour_node.add(node);
}
- if (is_valid(x - 1, y)) {
- NavigateNode node = new NavigateNode(x - 1, y);
- node.setNodeValue(map[x - 1][y]);
+ }else if(direction.equals("bottom")) {
+ NavigateNode node = getValidNavigateNode(map, x + 1, y);
+ if(node != null) {
+ neighbour_node.add(node);
+ }
+ }else if(direction.equals("left")) {
+ NavigateNode node = getValidNavigateNode(map, x, y - 1);
+ if(node != null) {
+ neighbour_node.add(node);
+ }
+ }else if(direction.equals("right")) {
+ NavigateNode node = getValidNavigateNode(map, x, y + 1);
+ if(node != null) {
neighbour_node.add(node);
}
}
@@ -251,22 +311,48 @@
return neighbour_node;
}
- public boolean is_valid(int x, int y) {
- if (x < 0 || x >= this.map.length
- || y < 0 || y >= this.map[0].length) {
- return false;
- }
- // 濡傛灉缁撶偣鐨勪綅缃皬浜�0锛屽垯涓嶅悎娉�
- if (map[x][y] < 0) {
- return false;
+ public NavigateNode getValidNavigateNode(List<List<NavigateNode>> map, int x, int y) {
+ if (x < 0 || x >= map.size()
+ || y < 0 || y >= map.get(0).size()) {
+ return null;
}
- if (map[x][y] == MapNodeType.CAR.id) {//鑺傜偣鏄皬杞�
- return false;
+ NavigateNode node = map.get(x).get(y);
+ if(node.getValue() == MapNodeType.DISABLE.id) {
+ return null;
}
- //浠ヤ笂鎯呭喌閮芥病鏈夊垯鍚堟硶
- return true;
+ return node;
+ }
+
+ public NavigateNode findStationNavigateNode(List<List<NavigateNode>> map, int stationId) {
+ for(int x = 0; x < map.size(); x++) {
+ for(int y = 0; y < map.get(0).size(); y++) {
+ NavigateNode node = map.get(x).get(y);
+ if("devp".equals(node.getNodeType())) {
+ JSONObject valueObj = JSON.parseObject(node.getNodeValue());
+ if(valueObj.getInteger("stationId") == stationId) {
+ return node;
+ }
+ }
+ }
+ }
+ return null;
+ }
+
+ public NavigateNode findTrackSiteNoNavigateNode(List<List<NavigateNode>> map, int trackSiteNo) {
+ for(int x = 0; x < map.size(); x++) {
+ for(int y = 0; y < map.get(0).size(); y++) {
+ NavigateNode node = map.get(x).get(y);
+ if("rgv".equals(node.getNodeType())) {
+ JSONObject valueObj = JSON.parseObject(node.getNodeValue());
+ if(valueObj.getInteger("trackSiteNo") == trackSiteNo) {
+ return node;
+ }
+ }
+ }
+ }
+ return null;
}
//------------------A*鍚彂鍑芥暟------------------//
@@ -300,4 +386,4 @@
//------------------A*鍚彂鍑芥暟-end------------------//
-}
\ No newline at end of file
+}
--
Gitblit v1.9.1