package com.zy.acs.manager.core.service;
|
|
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
|
import com.zy.acs.common.utils.RedisSupport;
|
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.service.astart.*;
|
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.Code;
|
import com.zy.acs.manager.manager.entity.Jam;
|
import com.zy.acs.manager.manager.entity.Segment;
|
import com.zy.acs.manager.manager.enums.JamStateType;
|
import com.zy.acs.manager.manager.service.*;
|
import com.zy.acs.manager.system.service.ConfigService;
|
import lombok.extern.slf4j.Slf4j;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.stereotype.Service;
|
|
import java.util.*;
|
import java.util.concurrent.CopyOnWriteArrayList;
|
import java.util.stream.Collectors;
|
|
/**
|
* Created by vincent on 7/25/2024
|
*/
|
@Slf4j
|
@Service
|
public class RetreatNavigateService {
|
|
private final RedisSupport redis = RedisSupport.defaultRedisSupport;
|
|
public static final int WEIGHT_CALC_FACTOR = 10000;
|
|
@Autowired
|
private CodeService codeService;
|
@Autowired
|
private MapDataDispatcher mapDataDispatcher;
|
@Autowired
|
private MapService mapService;
|
@Autowired
|
private AgvService agvService;
|
@Autowired
|
private AgvModelService agvModelService;
|
@Autowired
|
private LaneService laneService;
|
@Autowired
|
private ConfigService configService;
|
@Autowired
|
private SegmentService segmentService;
|
@Autowired
|
private JamService jamService;
|
|
/**
|
* avoidPathList ===>> [ minor vehicle ] [wave] [ curr vehicle ] [ code2 ] [ code3 ] ......
|
**/
|
// @SuppressWarnings("all")
|
public RetreatNavigateNode execute(String agvNo, RetreatNavigateNode start, List<String> avoidPathList, String sponsor, Jam jam) {
|
if (Cools.isEmpty(avoidPathList)) {
|
return null;
|
}
|
Integer lev = null;
|
Integer maxAgvCountInLane = configService.getVal("maxAgvCountInLane", Integer.class);
|
|
String breakPoint = avoidPathList.stream().findFirst().orElse(null);
|
List<String> blackList = Utils.singletonList(sponsor);
|
|
Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance(agvModelService.getByAgvNo(sponsor).getDiameter()
|
, MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
|
List<String> avoidPathListWave = mapService.getWaveScopeByCodeList(lev, avoidPathList, avoidDistance);
|
|
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
|
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
|
String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(lev);
|
|
|
RetreatNavigateNode finialNode = null;
|
|
PriorityQueue<RetreatNavigateNode> openQueue = new PriorityQueue<>();
|
Set<RetreatNavigateNode> existNodes = new HashSet<>();
|
|
openQueue.add(start);
|
existNodes.add(start);
|
boolean phaseSecond = true;
|
|
while (!openQueue.isEmpty() && null == finialNode) {
|
|
RetreatNavigateNode currentNode = openQueue.poll();
|
|
List<RetreatNavigateNode> enableNodes = new ArrayList<>();
|
|
List<RetreatNavigateNode> neighborNodes = this.getNeighborNodes(currentNode, existNodes, codeMatrix);
|
boolean pointOfTurn = neighborNodes.size() >= 2;
|
label: for (RetreatNavigateNode node : neighborNodes) {
|
if (node.getCodeData().equals(breakPoint)) { continue; }
|
|
int weight = 0;
|
|
// wave
|
String waveNode = waveMatrix[node.getX()][node.getY()];
|
assert !waveNode.equals(WaveNodeType.DISABLE.val);
|
if (!waveNode.equals(WaveNodeType.ENABLE.val)) {
|
List<String> waveNodeList = MapDataUtils.parseWaveNode(waveNode);
|
List<String> otherWaveList = MapDataUtils.hasOtherWave(waveNodeList, agvNo);
|
for (String otherWave : otherWaveList) {
|
if (!Cools.isEmpty(blackList) && blackList.contains(otherWave)) {
|
continue label;
|
}
|
if (mapService.isWalkingByVehicle(lev, otherWave)) {
|
phaseSecond = false; // there is a running way
|
continue label;
|
} else {
|
weight += WEIGHT_CALC_FACTOR;
|
}
|
}
|
}
|
|
// lane
|
if (pointOfTurn) {
|
List<int[]> laneCodeIdxList = laneService.getLaneCodeIdxList(node.getCodeData());
|
if (!Cools.isEmpty(laneCodeIdxList)) {
|
Set<String> lanVehicleSet = new HashSet<>();
|
|
for (int[] codeMatrixIdx : laneCodeIdxList) {
|
// overlap with sponsor
|
String laneCodeData = codeMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
|
if (avoidPathList.contains(laneCodeData)) {
|
lanVehicleSet.add(sponsor);
|
}
|
|
DynamicNode laneDynamicNode = dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
|
String laneVehicle = laneDynamicNode.getVehicle();
|
assert !laneVehicle.equals(DynamicNodeType.BLOCK.val);
|
if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) {
|
if (!laneVehicle.equals(agvNo)) {
|
lanVehicleSet.add(laneVehicle);
|
}
|
}
|
}
|
if (lanVehicleSet.size() + 1 > maxAgvCountInLane) {
|
phaseSecond = false; // there is a running way
|
continue;
|
}
|
if (lanVehicleSet.contains(sponsor)) {
|
weight += WEIGHT_CALC_FACTOR * 2;
|
}
|
}
|
|
}
|
|
Code code = codeService.getCacheByData(node.getCodeData());
|
|
// judge whether the node has already been marked as a retreat node?
|
// This is a very troublesome matter, if the node be repeatedly mark as a retreat node
|
List<Segment> runningSegments = segmentService.getRunningByEndCode(code.getId());
|
for (Segment runningSeg : runningSegments) {
|
if (0 < jamService.count(new LambdaQueryWrapper<Jam>()
|
.eq(Jam::getAvoSeg, runningSeg.getId())
|
.ne(Jam::getState, JamStateType.DEPRECATED.toString()))) {
|
weight += WEIGHT_CALC_FACTOR * 3;
|
} else {
|
weight += WEIGHT_CALC_FACTOR;
|
}
|
}
|
|
// enable
|
if (!avoidPathListWave.contains(node.getCodeData())) {
|
enableNodes.add(node);
|
}
|
|
int gCost = calcNodeCost(currentNode, node);
|
|
//进行计算对 G, F, H 等值
|
node.setLastDistance(gCost);
|
node.initNode(currentNode, weight);
|
|
openQueue.add(node);
|
existNodes.add(node);
|
|
}
|
|
if (!Cools.isEmpty(enableNodes)) {
|
Collections.sort(enableNodes);
|
finialNode = enableNodes.stream().findFirst().orElse(null);
|
jam.setCycleAvo(0);
|
}
|
}
|
|
|
if (null == finialNode && phaseSecond) {
|
// assert openQueue.size() == 0;
|
|
existNodes.clear();
|
openQueue.add(start);
|
existNodes.add(start);
|
|
RetreatNavigateNode availablePointOfTurn = null;
|
List<String> availablePointWaveScopeOfTurn = new ArrayList<>();
|
int actualLanesOfTurn = 0;
|
int filterPointOfTurnTimes = 0;
|
|
while (!openQueue.isEmpty() && null == finialNode) {
|
|
RetreatNavigateNode currentNode = openQueue.poll();
|
List<RetreatNavigateNode> enableNodes = new ArrayList<>();
|
|
List<RetreatNavigateNode> neighborNodes = this.getNeighborNodes(currentNode, existNodes, codeMatrix);
|
|
// 第一步：获取有效转弯点
|
if (null == availablePointOfTurn) {
|
// 计算是否为可用转弯点
|
if (neighborNodes.size() >= 2 && !isSame(start, currentNode)) {
|
filterPointOfTurnTimes ++;
|
if (filterPointOfTurnTimes > 2) { break; }
|
|
for (RetreatNavigateNode node : neighborNodes) {
|
// lane
|
List<int[]> laneCodeIdxList = laneService.getLaneCodeIdxList(node.getCodeData());
|
if (!Cools.isEmpty(laneCodeIdxList)) {
|
Set<String> lanVehicleSet = new HashSet<>();
|
|
for (int[] codeMatrixIdx : laneCodeIdxList) {
|
DynamicNode laneDynamicNode = dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
|
String laneVehicle = laneDynamicNode.getVehicle();
|
assert !laneVehicle.equals(DynamicNodeType.BLOCK.val);
|
if (!laneVehicle.equals(DynamicNodeType.ACCESS.val)) {
|
if (!laneVehicle.equals(agvNo)) {
|
lanVehicleSet.add(laneVehicle);
|
}
|
}
|
}
|
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
|
String waveNode = waveMatrix[node.getX()][node.getY()];
|
assert !waveNode.equals(WaveNodeType.DISABLE.val);
|
if (!waveNode.equals(WaveNodeType.ENABLE.val)) {
|
List<String> waveNodeList = MapDataUtils.parseWaveNode(waveNode);
|
List<String> otherWaveList = MapDataUtils.hasOtherWave(waveNodeList, agvNo);
|
for (String otherWave : otherWaveList) {
|
if (!Cools.isEmpty(blackList) && blackList.contains(otherWave)) {
|
continue;
|
}
|
if (mapService.isWalkingByVehicle(lev, otherWave)) {
|
|
if (null != availablePointOfTurn && actualLanesOfTurn > 0) {
|
actualLanesOfTurn --;
|
}
|
continue label;
|
} else {
|
weight += WEIGHT_CALC_FACTOR;
|
}
|
}
|
}
|
|
if (null != availablePointOfTurn) {
|
if (!availablePointWaveScopeOfTurn.contains(node.getCodeData())) {
|
enableNodes.add(node);
|
}
|
}
|
|
node.setLastDistance(calcNodeCost(currentNode, node));
|
node.initNode(currentNode, weight);
|
|
openQueue.add(node);
|
existNodes.add(node);
|
}
|
|
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);
|
}
|
}
|
|
}
|
|
}
|
|
return finialNode;
|
}
|
|
// 获取四周节点
|
private List<RetreatNavigateNode> getNeighborNodes(RetreatNavigateNode currentNode, Set<RetreatNavigateNode> existNodes, String[][] codeMatrix) {
|
|
int x = currentNode.getX();
|
int y = currentNode.getY();
|
|
List<RetreatNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
|
|
List<RetreatNavigateNode> possibleNodes = Arrays.asList(
|
new RetreatNavigateNode(x, y + 1), // right
|
new RetreatNavigateNode(x, y - 1), // left
|
new RetreatNavigateNode(x - 1, y), // up
|
new RetreatNavigateNode(x + 1, y) // down
|
);
|
|
possibleNodes.parallelStream()
|
.map(extendNode -> extendNeighborNodes(currentNode, extendNode, existNodes, null, null))
|
.filter(Objects::nonNull)
|
.peek(node -> node.setCodeData(codeMatrix[node.getX()][node.getY()]))
|
.forEach(neighbourNodes::add);
|
|
return neighbourNodes;
|
}
|
|
private RetreatNavigateNode extendNeighborNodes(RetreatNavigateNode currentNode, RetreatNavigateNode extendNode, Set<RetreatNavigateNode> existNodes, Integer dx, Integer dy) {
|
RetreatNavigateNode nextNode;
|
|
if (null == dx || null == dy) {
|
dx = extendNode.getX() - currentNode.getX();
|
dy = extendNode.getY() - currentNode.getY();
|
nextNode = extendNode;
|
} else {
|
nextNode = new RetreatNavigateNode(extendNode.getX() + dx, extendNode.getY() + dy);
|
}
|
|
int x = nextNode.getX();
|
int y = nextNode.getY();
|
|
// 数组越界
|
int[][] mapMatrix = mapDataDispatcher.getMapMatrix(null, null);
|
if (x < 0 || x >= mapMatrix.length
|
|| y < 0 || y >= mapMatrix[0].length) {
|
return null;
|
}
|
|
if (mapMatrix[x][y] == MapNodeType.DISABLE.val) {
|
|
return extendNeighborNodes(currentNode, nextNode, existNodes, dx, dy);
|
}
|
|
assert mapMatrix[x][y] == MapNodeType.ENABLE.val;
|
|
if (existNodes.contains(nextNode)) {
|
return null;
|
}
|
|
// 判断通过性
|
String routeCdaKey = RouteGenerator.generateRouteCdaKey(new int[]{currentNode.getX(), currentNode.getY()}, new int[]{nextNode.getX(), nextNode.getY()});
|
if (!mapDataDispatcher.validRouteCdaKey(routeCdaKey)) {
|
return null;
|
}
|
|
return nextNode;
|
}
|
|
private boolean isSame(RetreatNavigateNode o1, RetreatNavigateNode o2) {
|
if (Cools.isEmpty(o1, o2)) {
|
return false;
|
}
|
return o1.getX() == o2.getX() && o1.getY() == o2.getY();
|
}
|
|
private int calcNodeCost(RetreatNavigateNode node1, RetreatNavigateNode node2) {
|
|
return Math.abs(node2.getX() - node1.getX()) + Math.abs(node2.getY() - node1.getY());
|
}
|
|
}
|
