From 0f2eaeeb387850acfede95c5c061b5392dae8688 Mon Sep 17 00:00:00 2001
From: Junjie <fallin.jie@qq.com>
Date: 星期三, 28 六月 2023 16:58:25 +0800
Subject: [PATCH] 寻路算法和避障
---
src/main/java/com/zy/asrs/service/impl/MainServiceImpl.java | 147 +++++++++++++++++++++++++++++-------------------
1 files changed, 89 insertions(+), 58 deletions(-)
diff --git a/src/main/java/com/zy/asrs/service/impl/MainServiceImpl.java b/src/main/java/com/zy/asrs/service/impl/MainServiceImpl.java
index cfede0a..3cecf33 100644
--- a/src/main/java/com/zy/asrs/service/impl/MainServiceImpl.java
+++ b/src/main/java/com/zy/asrs/service/impl/MainServiceImpl.java
@@ -571,7 +571,7 @@
//鏁版嵁搴撲腑涔熶笉瀛樺湪鍦板浘鏁版嵁锛屼粠鍦板浘鏂囦欢涓幏鍙�
//杞藉叆鍦板浘
NavigateMapData mapData = new NavigateMapData(i);
- List<List<MapNode>> lists = mapData.getJsonData(-1, null);//鑾峰彇瀹屾暣鍦板浘(鍖呮嫭鍏ュ簱鍑哄簱)
+ List<List<MapNode>> lists = mapData.getJsonData(-1, null, null);//鑾峰彇瀹屾暣鍦板浘(鍖呮嫭鍏ュ簱鍑哄簱)
//瀛樺叆鏁版嵁搴�
basMap = new BasMap();
@@ -826,12 +826,14 @@
}
//璁＄畻灏忚溅璧风偣鍒颁腑鐐规墍闇�鍛戒护
- List<NavigateNode> calc = NavigateUtils.calc(startLocNo, locNo, mapType);
+ List<NavigateNode> calc = NavigateUtils.calc(startLocNo, locNo, mapType, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), Utils.getLev(startLocNo)));
List<ShuttleCommand> commands = new ArrayList<>();
if (calc == null) {
return null;
}
-
+ if (!Utils.checkShuttlePath(calc, shuttleThread.getSlave().getId())) {//妫�娴嬬┛姊溅琛岃蛋璺緞锛屾槸鍚﹀瓨鍦ㄥ叾浠栧皬杞︼紝濡傛湁鍏朵粬灏忚溅鍒欒繘琛岃皟绂�
+ return null;
+ }
List<NavigateNode> allNode = new ArrayList<>();
allNode.addAll(calc);
@@ -881,10 +883,14 @@
List<NavigateNode> allNode = new ArrayList<>();
//璁＄畻灏忚溅璧风偣鍒颁腑鐐规墍闇�鍛戒护
- List<NavigateNode> calc = NavigateUtils.calc(startLocNo, middleLocNo, NavigationMapType.NORMAL.id);//灏忚溅鏃犺揣锛岃蛋姝ｅ父搴撲綅閫氶亾
+ List<NavigateNode> calc = NavigateUtils.calc(startLocNo, middleLocNo, NavigationMapType.NORMAL.id, null);//灏忚溅鏃犺揣锛岃蛋姝ｅ父搴撲綅閫氶亾
List<ShuttleCommand> commands = new ArrayList<>();
if (calc != null) {
+ if (!Utils.checkShuttlePath(calc, shuttleThread.getSlave().getId())) {//妫�娴嬬┛姊溅琛岃蛋璺緞锛屾槸鍚﹀瓨鍦ㄥ叾浠栧皬杞︼紝濡傛湁鍏朵粬灏忚溅鍒欒繘琛岃皟绂�
+ return null;
+ }
+
allNode.addAll(calc);
//鑾峰彇鍒嗘璺緞
ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
@@ -915,11 +921,13 @@
commands.add(shuttleThread.getPalletCommand((short) 1));
//璁＄畻灏忚溅涓偣鍒扮粓鐐规墍闇�鍛戒护
- List<NavigateNode> calc2 = NavigateUtils.calc(middleLocNo, locNo, NavigationMapType.DFX.id);//灏忚溅鏈夎揣锛岃蛋绂佺敤杩嘍FX搴撲綅鐨勫湴鍥鹃�氶亾
+ List<NavigateNode> calc2 = NavigateUtils.calc(middleLocNo, locNo, NavigationMapType.DFX.id, null);//灏忚溅鏈夎揣锛岃蛋绂佺敤杩嘍FX搴撲綅鐨勫湴鍥鹃�氶亾
if (calc2 == null) {
return null;
}
-
+ if (!Utils.checkShuttlePath(calc2, shuttleThread.getSlave().getId())) {//妫�娴嬬┛姊溅琛岃蛋璺緞锛屾槸鍚﹀瓨鍦ㄥ叾浠栧皬杞︼紝濡傛湁鍏朵粬灏忚溅鍒欒繘琛岃皟绂�
+ return null;
+ }
allNode.addAll(calc2);
//鑾峰彇鍒嗘璺緞
@@ -1006,8 +1014,11 @@
//鍒ゆ柇鎻愬崌鏈烘ゼ灞傛槸鍚﹀埌浣嶏紝鍒ゆ柇绔欑偣鏄惁缁欏嚭鎻愬崌鏈哄埌浣嶄俊鍙�
String locNo = wrkMast.getSourceLocNo();
int lev = Utils.getLev(locNo);//鐩爣浜岀淮鐮佹墍鍦ㄦゼ灞�
- int liftLev = liftProtocol.getLev().intValue();//鎻愬崌鏈烘墍鍦ㄦゼ灞�
- if (liftLev != lev) {
+ Short liftLev = liftProtocol.getLev();//鎻愬崌鏈烘墍鍦ㄦゼ灞�
+ if (liftLev == null) {//鎻愬崌鏈哄彲鑳藉湪杈撻�佺嚎妤煎眰
+ continue;
+ }
+ if (liftLev.intValue() != lev) {
continue;//鎻愬崌鏈轰笉鍦ㄧ洰鏍囨ゼ灞傝烦杩�
}
@@ -1069,25 +1080,32 @@
//鍒ゆ柇灏忚溅鍜屽簱浣嶆槸鍚﹀湪鍚屼竴灞�
if (Boolean.parseBoolean(searchIdleShuttle.get("sameLay").toString())) {
//鍚屼竴灞�(灏嗗皬杞︾Щ鍔ㄥ埌璐х墿浣嶇疆)
+
List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, wrkMast.getSourceLocNo(), liftSiteLocNo, assignCommand, shuttleThread);
if (commands == null) {
//鏈壘鍒拌矾寰勶紝绛夊緟涓嬩竴娆�
continue;
}
+ //鎵�浣跨敤鐨勮矾寰勮繘琛屼复鏃惰В閿侊紝鐢ㄤ簬鍚庣画璁＄畻
+ NavigateMapData navigateMapData = new NavigateMapData(currentLev);
+ navigateMapData.writeNavigateNodeToRedisMap(assignCommand.getNodes(), false);//鎵�浣跨敤鐨勮矾寰勮繘琛屼复鏃惰В閿�
- //鑾峰彇褰撳墠灏忚溅鎵�鍦ㄦゼ灞傜殑绔欑偣淇℃伅
- BasDevp basDevp = basDevpService.queryByLocNo(liftSiteLocNo);
- Short endStartCode = Short.parseShort(basDevp.getQrCodeValue());//绔欑偣浜岀淮鐮�
-
- String disLocNo = "200020" + Utils.getLev(liftSiteLocNo);//閬胯浣嶇疆
- LocMast locMast1 = locMastService.queryByLoc(disLocNo);
- if (locMast1 == null) {
- continue;//鎵句笉鍒板簱浣�
- }
- short disCode = Short.parseShort(locMast1.getQrCodeValue());
//浠诲姟鎵ц瀹屽悗锛屽皬杞﹁繘鍏ョЩ寮�鎻愬崌鏈哄彛绔欑偣浣嶇疆锛屼互鍏嶅潬钀�
- ShuttleCommand moveCommand = shuttleThread.getMoveCommand(endStartCode, disCode, 2800, ShuttleRunDirection.BOTTOM.id, endStartCode, 2800, 1000);
- commands.add(moveCommand);
+ //鎼滅储涓�鏉℃病鏈夊皬杞︾殑绌哄贩閬擄紝骞惰皟搴﹀皬杞�
+ int distLev = Utils.getLev(liftSiteLocNo);//閬胯妤煎眰
+ String startLocNo = "180020" + Utils.getLev(liftSiteLocNo);
+ ShuttleAssignCommand moveAssignCommand = Utils.searchEmptyGroupToMoveShuttle(distLev, shuttleThread.getSlave().getId(), shuttleThread, startLocNo);
+ if (moveAssignCommand == null) {//璋冨害灏忚溅鍛戒护涓虹┖
+ continue;
+ }
+ //鎵�浣跨敤鐨勮矾寰勮繘琛屼复鏃惰В閿侊紝鐢ㄤ簬鍚庣画璁＄畻
+ navigateMapData.writeNavigateNodeToRedisMap(moveAssignCommand.getNodes(), false);//鎵�浣跨敤鐨勮矾寰勮繘琛屼复鏃惰В閿�
+ commands.addAll(moveAssignCommand.getCommands());//灏嗛伩璁╁皬杞︾殑鍛戒护娣诲姞
+ List<NavigateNode> nodes = assignCommand.getNodes();//灏嗛伩璁╄矾寰勬坊鍔犺繘鑺傜偣璺緞涓�
+ nodes.addAll(moveAssignCommand.getNodes());//灏嗛伩璁╄矾寰勬坊鍔犺繘鑺傜偣璺緞涓�
+ assignCommand.setNodes(nodes);
+ navigateMapData.writeNavigateNodeToRedisMap(nodes, true);//鎵�浣跨敤鐨勮矾寰勮繘琛岄攣瀹�
+
//鍒嗛厤鐩爣搴撲綅
shuttleProtocol.setLocNo(wrkMast.getSourceLocNo());
@@ -1155,24 +1173,30 @@
assignCommand.setTaskMode(ShuttleTaskModeType.PAK_OUT.id.shortValue());
assignCommand.setSourceLocNo(liftSiteLocNo);
- //鑾峰彇褰撳墠灏忚溅鎵�鍦ㄦゼ灞傜殑绔欑偣淇℃伅
- BasDevp basDevp = basDevpService.queryByLocNo(liftSiteLocNo);
- Short endStartCode = Short.parseShort(basDevp.getQrCodeValue());//绔欑偣浜岀淮鐮�
-
- String disLocNo = "200020" + Utils.getLev(liftSiteLocNo);//閬胯浣嶇疆
- LocMast locMast1 = locMastService.queryByLoc(disLocNo);
- if (locMast1 == null) {
- continue;//鎵句笉鍒板簱浣�
- }
- short disCode = Short.parseShort(locMast1.getQrCodeValue());
- //浠诲姟鎵ц瀹屽悗锛屽皬杞﹁繘鍏ョЩ寮�鎻愬崌鏈哄彛绔欑偣浣嶇疆锛屼互鍏嶅潬钀�
- ShuttleCommand moveCommand2 = shuttleThread.getMoveCommand(endStartCode, disCode, 2800, ShuttleRunDirection.BOTTOM.id, endStartCode, 2800, 1000);
-
List<ShuttleCommand> commands = this.shuttleAssignCommand(liftSiteLocNo, wrkMast.getSourceLocNo(), liftSiteLocNo, assignCommand, shuttleThread);
if (commands == null) {
continue;//鏈壘鍒拌矾寰�
}
- commands.add(moveCommand2);//浠诲姟鎵ц瀹屽悗锛屽皬杞﹁繘鍏ョЩ寮�鎻愬崌鏈哄彛绔欑偣浣嶇疆锛屼互鍏嶅潬钀�
+ //鎵�浣跨敤鐨勮矾寰勮繘琛屼复鏃惰В閿侊紝鐢ㄤ簬鍚庣画璁＄畻
+ NavigateMapData navigateMapData = new NavigateMapData(Utils.getLev(liftSiteLocNo));
+ navigateMapData.writeNavigateNodeToRedisMap(assignCommand.getNodes(), false);//鎵�浣跨敤鐨勮矾寰勮繘琛屼复鏃惰В閿�
+
+ //浠诲姟鎵ц瀹屽悗锛屽皬杞﹁繘鍏ョЩ寮�鎻愬崌鏈哄彛绔欑偣浣嶇疆锛屼互鍏嶅潬钀�
+ //鎼滅储涓�鏉℃病鏈夊皬杞︾殑绌哄贩閬擄紝骞惰皟搴﹀皬杞�
+ int distLev = Utils.getLev(liftSiteLocNo);//閬胯妤煎眰
+ String startLocNo = "180020" + Utils.getLev(liftSiteLocNo);
+ ShuttleAssignCommand moveAssignCommand = Utils.searchEmptyGroupToMoveShuttle(distLev, shuttleThread.getSlave().getId(), shuttleThread, startLocNo);
+ if (moveAssignCommand == null) {//璋冨害灏忚溅鍛戒护涓虹┖
+ continue;
+ }
+ //鎵�浣跨敤鐨勮矾寰勮繘琛屼复鏃惰В閿侊紝鐢ㄤ簬鍚庣画璁＄畻
+ navigateMapData.writeNavigateNodeToRedisMap(moveAssignCommand.getNodes(), false);//鎵�浣跨敤鐨勮矾寰勮繘琛屼复鏃惰В閿�
+ commands.addAll(moveAssignCommand.getCommands());//灏嗛伩璁╁皬杞︾殑鍛戒护娣诲姞
+ List<NavigateNode> nodes = assignCommand.getNodes();//灏嗛伩璁╄矾寰勬坊鍔犺繘鑺傜偣璺緞涓�
+ nodes.addAll(moveAssignCommand.getNodes());//灏嗛伩璁╄矾寰勬坊鍔犺繘鑺傜偣璺緞涓�
+ assignCommand.setNodes(nodes);
+ navigateMapData.writeNavigateNodeToRedisMap(nodes, true);//鎵�浣跨敤鐨勮矾寰勮繘琛岄攣瀹�
+
//姝ゆ椂杞﹀湪鎻愬崌鏈哄唴閮紝闇�瑕佸涓嬭揪涓�姝ユ寚浠よ杞︾Щ鍔ㄥ埌鎻愬崌鏈哄彛
short startCode = liftProtocol.getBarcode();//鎻愬崌鏈哄唴閮ㄤ簩缁寸爜
@@ -1274,9 +1298,9 @@
String recentLocNo = recentShuttle.getShuttleProtocol().getCurrentLocNo();
//褰撳墠鏈�杩戝洓鍚戠┛姊溅鍒扮洰鏍囧湴鐐硅窛绂�
- List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, distLocNo, NavigationMapType.NORMAL.id);//鎼滅储绌洪棽绌挎杞︼紝浣跨敤姝ｅ父閫氶亾鍦板浘
+ List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, distLocNo, NavigationMapType.NORMAL.id, Utils.getShuttlePoints(recentShuttle.getSlave().getId(), Utils.getLev(recentLocNo)));//鎼滅储绌洪棽绌挎杞︼紝浣跨敤姝ｅ父閫氶亾鍦板浘
//褰撳墠绌挎杞︾嚎绋嬪埌鐩爣鍦扮偣璺濈
- List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, distLocNo, NavigationMapType.NORMAL.id);//鎼滅储绌洪棽绌挎杞︼紝浣跨敤姝ｅ父閫氶亾鍦板浘
+ List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, distLocNo, NavigationMapType.NORMAL.id, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), Utils.getLev(currentLocNo)));//鎼滅储绌洪棽绌挎杞︼紝浣跨敤姝ｅ父閫氶亾鍦板浘
if (recentShuttlePath == null || currentShuttlePath == null) {
continue;
}
@@ -1306,9 +1330,9 @@
int recentLev = Utils.getLev(recentLocNo);
//褰撳墠鏈�杩戝洓鍚戠┛姊溅鍒板綋鍓嶈溅瀛愭墍鍦ㄦゼ灞傜殑鎻愬崌鏈哄彛璺濈
- List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, Utils.levToOutInStaLocNo(recentLev), NavigationMapType.NORMAL.id);//鎼滅储绌洪棽绌挎杞︼紝浣跨敤姝ｅ父閫氶亾鍦板浘
+ List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, Utils.levToOutInStaLocNo(recentLev), NavigationMapType.NORMAL.id, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), recentLev));//鎼滅储绌洪棽绌挎杞︼紝浣跨敤姝ｅ父閫氶亾鍦板浘
//褰撳墠绌挎杞︾嚎绋嬪埌褰撳墠杞﹀瓙鎵�鍦ㄦゼ灞傜殑鎻愬崌鏈哄彛璺濈
- List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, Utils.levToOutInStaLocNo(currentLev), NavigationMapType.NORMAL.id);//鎼滅储绌洪棽绌挎杞︼紝浣跨敤姝ｅ父閫氶亾鍦板浘
+ List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, Utils.levToOutInStaLocNo(currentLev), NavigationMapType.NORMAL.id, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), currentLev));//鎼滅储绌洪棽绌挎杞︼紝浣跨敤姝ｅ父閫氶亾鍦板浘
if (recentShuttlePath == null || currentShuttlePath == null) {
continue;
}
@@ -1740,8 +1764,6 @@
if (!result) {
throw new CoolException("鏇存柊plc绔欑偣淇℃伅澶辫触");
}
- //浠诲姟鍙锋竻闆�
- liftProtocol.setTaskNo((short) 0);
wrkMast.setWrkSts(4L);
break;
case 7://7.鎻愬崌鏈鸿縼绉诲皬杞︿腑 ==> 8.鎻愬崌鏈鸿縼绉诲皬杞﹀畬鎴�
@@ -1762,8 +1784,6 @@
}
wrkMast.setWrkSts(29L);
wrkMast.setWrkSts(34L);//34.鍑哄簱瀹屾垚锛屾殏鏃跺厛鐩存帴瀹屾垚鍑哄簱宸ヤ綔妗ｏ紝鍚庣画闇�瑕佹牴鎹緭閫佺嚎缁欏嚭鐨勭姸鎬佹潵纭畾34.鍑哄簱瀹屾垚鐘舵��
- //浠诲姟鍙锋竻闆�
- liftProtocol.setTaskNo((short) 0);
break;
default:
}
@@ -1773,6 +1793,8 @@
liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE);
//浠诲姟鎸囦护娓呴浂
liftProtocol.setAssignCommand(null);
+ //浠诲姟鍙锋竻闆�
+ liftProtocol.setTaskNo((short) 0);
News.info("鎻愬崌鏈哄凡纭涓斾换鍔″畬鎴愮姸鎬併�傛彁鍗囨満鍙�={}", liftProtocol.getLiftNo());
} else {
News.error("鎻愬崌鏈哄凡纭涓斾换鍔″畬鎴愮姸鎬�,澶嶄綅澶辫触锛屼絾鏈壘鍒板伐浣滄。銆傛彁鍗囨満鍙�={}锛屽伐浣滃彿={}", liftProtocol.getLiftNo(), liftProtocol.getTaskNo());
@@ -1811,8 +1833,11 @@
if (liftProtocol.getAssignCommand() != null) {
//璁剧疆鎻愬崌鏈轰负绌洪棽鐘舵��
liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE);
- //浠诲姟鍙锋竻闆�
- liftProtocol.setTaskNo((short) 0);
+ //鍒ゆ柇鏄惁涓哄洓鍚戠┛姊溅璋冨害鎻愬崌鏈猴紝濡傛槸鍒欐棤闇�娓呯悊浠诲姟鍙�
+ if (!liftProtocol.getSecurityMk()) {
+ //浠诲姟鍙锋竻闆�
+ liftProtocol.setTaskNo((short) 0);
+ }
//鏍囪澶嶄綅
liftProtocol.setPakMk(true);
//浠诲姟鎸囦护娓呴浂
@@ -2756,21 +2781,27 @@
//鍒ゆ柇灏忚溅鏄惁鍏呮弧鐢甸噺锛屾弧鐢�1000鎴栫數鍘�54V浠ヤ笂
if (shuttleProtocol.getBatteryPower() >= 1000 && shuttleProtocol.getCurrentVoltage() >= 54000) {
//鍏呮弧锛屾柇寮�鍏呯數
- List<ShuttleCommand> commands = new ArrayList<>();
- ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
- assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
- assignCommand.setTaskMode((short) 0);
- assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
- assignCommand.setCharge(true);
+// List<ShuttleCommand> commands = new ArrayList<>();
+// ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
+// assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
+// assignCommand.setTaskMode((short) 0);
+// assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
+// assignCommand.setCharge(true);
+//
+// //鍒涘缓鍏呯數鎸囦护
+// ShuttleCommand command = shuttleThread.getChargeSwitchCommand((short) 2);//鏂紑鍏呯數
+// commands.add(command);
+//
+// //鎸囦护闆嗗垎閰�
+// assignCommand.setCommands(commands);
+//
+// shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING);
- //鍒涘缓鍏呯數鎸囦护
- ShuttleCommand command = shuttleThread.getChargeSwitchCommand((short) 2);//鏂紑鍏呯數
- commands.add(command);
-
- //鎸囦护闆嗗垎閰�
- assignCommand.setCommands(commands);
-
- shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING);
+ //灏嗗皬杞︾Щ鍔ㄥ埌绌洪棽鐨勫贩閬�
+ ShuttleAssignCommand assignCommand = Utils.searchEmptyGroupToMoveShuttle(Utils.getLev(shuttleProtocol.getLocNo()), shuttleProtocol.getShuttleNo().intValue(), shuttleThread, null);
+ if (assignCommand == null) {
+ continue;
+ }
wrkCharge.setWrkSts(60L);//60.鍏呯數浠诲姟瀹屾垚
if (wrkChargeMapper.updateById(wrkCharge) > 0) {
--
Gitblit v1.9.1