From 04419e30ed2be12063a88883889bf1017591f3c4 Mon Sep 17 00:00:00 2001 From: luxiaotao1123 <t1341870251@163.com> Date: 星期四, 07 十一月 2024 09:07:01 +0800 Subject: [PATCH] # --- zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/RetreatNavigateService.java | 175 +++++++++++++++++++++++++++++++++------------------------ 1 files changed, 101 insertions(+), 74 deletions(-) diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/RetreatNavigateService.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/RetreatNavigateService.java index d52e290..6d89a53 100644 --- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/RetreatNavigateService.java +++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/RetreatNavigateService.java @@ -1,8 +1,8 @@ package com.zy.acs.manager.core.service; import com.zy.acs.common.utils.RedisSupport; -import com.zy.acs.framework.common.Cools; import com.zy.acs.common.utils.Utils; +import com.zy.acs.framework.common.Cools; import com.zy.acs.manager.common.utils.MapDataUtils; import com.zy.acs.manager.core.constant.MapDataConstant; import com.zy.acs.manager.core.domain.Lane; @@ -31,7 +31,7 @@ private final RedisSupport redis = RedisSupport.defaultRedisSupport; - public static final int WEIGHT_CALC_COEFFICIENT = 1000; + public static final int WEIGHT_CALC_FACTOR = 10000; @Autowired private CodeService codeService; @@ -79,6 +79,7 @@ openQueue.add(start); existNodes.add(start); + boolean phaseSecond = true; while (openQueue.size() > 0 && null == finialNode) { @@ -87,6 +88,7 @@ List<RetreatNavigateNode> enableNodes = new ArrayList<>(); ArrayList<RetreatNavigateNode> neighborNodes = this.getNeighborNodes(currentNode, existNodes); + boolean pointOfTurn = neighborNodes.size() >= 2; label: for (RetreatNavigateNode node : neighborNodes) { if (node.getCodeData().equals(breakPoint)) { continue; } @@ -103,35 +105,46 @@ continue label; } if (1 < mapDataDispatcher.queryCodeListFromDynamicNode(lev, otherWave).size()) { + phaseSecond = false; // there is a way to go continue label; } else { - weight += WEIGHT_CALC_COEFFICIENT; + weight += WEIGHT_CALC_FACTOR; } } } // lane - Lane lane = laneService.search(node.getCodeData()); - if (null != lane) { - Set<String> lanVehicleSet = new HashSet<>(); + if (pointOfTurn) { + Lane lane = laneService.search(node.getCodeData()); + if (null != lane) { + Set<String> lanVehicleSet = new HashSet<>(); - List<String> laneCodes = lane.getCodes(); - for (String laneCodeData : laneCodes) { - int[] laneCodeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(null, laneCodeData); - // scan dynamicMatrix or WaveMatrix - DynamicNode laneDynamicNode = dynamicMatrix[laneCodeMatrixIdx[0]][laneCodeMatrixIdx[1]]; - String laneVehicle = laneDynamicNode.getVehicle(); - assert !laneVehicle.equals(DynamicNodeType.BLOCK.val); - if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) { - if (!laneVehicle.equals(agvNo)) { - lanVehicleSet.add(laneVehicle); -// redis.setObject(RedisConstant.AGV_TO_STANDBY_FLAG, laneVehicle, true, 30); + for (String laneCodeData : lane.getCodes()) { + // overlap with sponsor + if (avoidPathList.contains(laneCodeData)) { + lanVehicleSet.add(sponsor); + } + + int[] laneCodeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(null, laneCodeData); + // scan dynamicMatrix or WaveMatrix + DynamicNode laneDynamicNode = dynamicMatrix[laneCodeMatrixIdx[0]][laneCodeMatrixIdx[1]]; + String laneVehicle = laneDynamicNode.getVehicle(); + assert !laneVehicle.equals(DynamicNodeType.BLOCK.val); + if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) { + if (!laneVehicle.equals(agvNo)) { + lanVehicleSet.add(laneVehicle); +// redis.setObject(RedisConstant.AGV_TO_STANDBY_FLAG, laneVehicle, true, 30); + } } } - } - if (lanVehicleSet.size() + 1 > maxAgvCountInLane) { - continue; + if (lanVehicleSet.size() + 1 > maxAgvCountInLane) { + phaseSecond = false; // there is a way to go + continue; + } + if (lanVehicleSet.contains(sponsor)) { + weight += WEIGHT_CALC_FACTOR * 2; + } } } @@ -159,15 +172,17 @@ } - if (null == finialNode) { + if (null == finialNode && phaseSecond) { // assert openQueue.size() == 0; existNodes.clear(); openQueue.add(start); existNodes.add(start); - RetreatNavigateNode firstPointOfTurn = null; - List<String> firstPointWaveScopeOfTurn = new ArrayList<>(); + RetreatNavigateNode availablePointOfTurn = null; + List<String> availablePointWaveScopeOfTurn = new ArrayList<>(); + int actualLanesOfTurn = 0; + int filterPointOfTurnTimes = 0; while (openQueue.size() > 0 && null == finialNode) { @@ -175,15 +190,55 @@ List<RetreatNavigateNode> enableNodes = new ArrayList<>(); ArrayList<RetreatNavigateNode> neighborNodes = this.getNeighborNodes(currentNode, existNodes); - int forks = neighborNodes.size(); - if (firstPointOfTurn == null && forks >= 2 && !isSame(start, currentNode)) { - firstPointOfTurn = currentNode; - firstPointWaveScopeOfTurn = mapService.getWaveScopeByCode(lev, firstPointOfTurn.getCodeData(), avoidDistance) - .stream().map(NavigateNode::getCodeData).distinct().collect(Collectors.toList()); + + // 绗竴姝ワ細鑾峰彇鏈夋晥杞集鐐� + if (null == availablePointOfTurn) { + // 璁$畻鏄惁涓哄彲鐢ㄨ浆寮偣 + if (neighborNodes.size() >= 2 && !isSame(start, currentNode)) { + filterPointOfTurnTimes ++; + if (filterPointOfTurnTimes > 2) { break; } + + for (RetreatNavigateNode node : neighborNodes) { + // lane + Lane lane = laneService.search(node.getCodeData()); + if (null != lane) { + Set<String> lanVehicleSet = new HashSet<>(); + + for (String laneCodeData : lane.getCodes()) { + int[] laneCodeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(null, laneCodeData); + // scan dynamicMatrix or WaveMatrix + DynamicNode laneDynamicNode = dynamicMatrix[laneCodeMatrixIdx[0]][laneCodeMatrixIdx[1]]; + String laneVehicle = laneDynamicNode.getVehicle(); + assert !laneVehicle.equals(DynamicNodeType.BLOCK.val); + if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) { + if (!laneVehicle.equals(agvNo)) { + lanVehicleSet.add(laneVehicle); +// redis.setObject(RedisConstant.AGV_TO_STANDBY_FLAG, laneVehicle, true, 30); + } + } + } + + if (lanVehicleSet.size() + 1 > maxAgvCountInLane) { + continue; + } + } + + actualLanesOfTurn ++; + } + + // 鏈変袱鏉′互涓婂彲璧板贩閬擄紝鍒欒涓烘湁鏁堣浆寮偣 + if (actualLanesOfTurn >= 2) { + availablePointOfTurn = currentNode; + availablePointWaveScopeOfTurn = mapService.getWaveScopeByCode(lev, availablePointOfTurn.getCodeData(), avoidDistance) + .stream().map(NavigateNode::getCodeData).distinct().collect(Collectors.toList()); + } else { + actualLanesOfTurn = 0; + } + } } + // 寤朵几杞集鐐瑰贩閬� label: for (RetreatNavigateNode node : neighborNodes) { - int weight = 0; // wave @@ -197,41 +252,19 @@ continue; } if (1 < mapDataDispatcher.queryCodeListFromDynamicNode(lev, otherWave).size()) { + + if (null != availablePointOfTurn && actualLanesOfTurn > 0) { + actualLanesOfTurn --; + } continue label; } else { - weight += WEIGHT_CALC_COEFFICIENT; + weight += WEIGHT_CALC_FACTOR; } } } - // lane - Lane lane = laneService.search(node.getCodeData()); - if (null != lane) { - Set<String> lanVehicleSet = new HashSet<>(); - - List<String> laneCodes = lane.getCodes(); - for (String laneCodeData : laneCodes) { - int[] laneCodeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(null, laneCodeData); - // scan dynamicMatrix or WaveMatrix - DynamicNode laneDynamicNode = dynamicMatrix[laneCodeMatrixIdx[0]][laneCodeMatrixIdx[1]]; - String laneVehicle = laneDynamicNode.getVehicle(); - assert !laneVehicle.equals(DynamicNodeType.BLOCK.val); - if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) { - if (!laneVehicle.equals(agvNo)) { - lanVehicleSet.add(laneVehicle); -// redis.setObject(RedisConstant.AGV_TO_STANDBY_FLAG, laneVehicle, true, 30); - } - } - } - - if (lanVehicleSet.size() + 1 > maxAgvCountInLane) { - continue; - } - } - - // have cross turn road - if (null != firstPointOfTurn) { - if (!firstPointWaveScopeOfTurn.contains(node.getCodeData())) { + if (null != availablePointOfTurn) { + if (!availablePointWaveScopeOfTurn.contains(node.getCodeData())) { enableNodes.add(node); } } @@ -243,10 +276,16 @@ existNodes.add(node); } - if (!Cools.isEmpty(enableNodes)) { - Collections.sort(enableNodes); - finialNode = enableNodes.stream().findFirst().orElse(null); - jam.setCycleAvo(1); + if (actualLanesOfTurn < 2) { + availablePointOfTurn = null; + availablePointWaveScopeOfTurn = new ArrayList<>(); + actualLanesOfTurn = 0; + } else { + if (!Cools.isEmpty(enableNodes)) { + Collections.sort(enableNodes); + finialNode = enableNodes.stream().findFirst().orElse(null); + jam.setCycleAvo(1); + } } } @@ -254,18 +293,6 @@ } return finialNode; - } - - private int calcNodeWeightVal(List<String> otherWaveList) { - int weightVal = 0; - - if (!Cools.isEmpty(otherWaveList)) { - - - } - - - return weightVal; } // 鑾峰彇鍥涘懆鑺傜偣 -- Gitblit v1.9.1