package com.zy.acs.manager.core.service;
|
|
import com.zy.acs.framework.common.Cools;
|
import com.zy.acs.common.utils.Utils;
|
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.manager.entity.Jam;
|
import com.zy.acs.manager.manager.entity.Route;
|
import com.zy.acs.manager.manager.service.AgvModelService;
|
import com.zy.acs.manager.manager.service.AgvService;
|
import com.zy.acs.manager.manager.service.CodeService;
|
import com.zy.acs.manager.manager.service.RouteService;
|
import lombok.extern.slf4j.Slf4j;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.stereotype.Service;
|
|
import java.util.ArrayList;
|
import java.util.Collections;
|
import java.util.List;
|
import java.util.PriorityQueue;
|
import java.util.stream.Collectors;
|
|
/**
|
* Created by vincent on 7/25/2024
|
*/
|
@Slf4j
|
@Service
|
public class RetreatNavigateService {
|
|
@Autowired
|
private CodeService codeService;
|
@Autowired
|
private RouteService routeService;
|
@Autowired
|
private MapDataDispatcher mapDataDispatcher;
|
@Autowired
|
private MapService mapService;
|
@Autowired
|
private AgvService agvService;
|
@Autowired
|
private AgvModelService agvModelService;
|
|
/**
|
* 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;
|
|
String breakPoint = avoidPathList.stream().findFirst().orElse(null);
|
List<String> blackList = Utils.singletonList(sponsor);
|
|
Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance(agvModelService.getById(agvService.selectByUuid(sponsor).getAgvModel()).getDiameter()
|
, MapDataConstant.IDLE_DISTANCE_COE);
|
List<String> avoidPathListWave = mapService.getWaveScopeByCodeList(lev, avoidPathList, avoidDistance);
|
|
String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(lev);
|
|
|
|
RetreatNavigateNode finialNode = null;
|
|
PriorityQueue<RetreatNavigateNode> openQueue = new PriorityQueue<>();
|
ArrayList<RetreatNavigateNode> existNodes = new ArrayList<>();
|
|
openQueue.add(start);
|
existNodes.add(start);
|
|
while (openQueue.size() > 0 && null == finialNode) {
|
|
RetreatNavigateNode currentNode = openQueue.poll();
|
|
List<RetreatNavigateNode> enableNodes = new ArrayList<>();
|
|
ArrayList<RetreatNavigateNode> neighborNodes = this.getNeighborNodes(currentNode, existNodes);
|
for (RetreatNavigateNode node : neighborNodes) {
|
if (node.getCodeData().equals(breakPoint)) { continue; }
|
|
int weight = 0;
|
|
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);
|
if (!Cools.isEmpty(otherWaveList)) {
|
|
if (!Cools.isEmpty(blackList) && !Cools.isEmpty(MapDataUtils.hasIntersection(otherWaveList, blackList))) {
|
continue;
|
}
|
|
weight += otherWaveList.size();
|
}
|
}
|
|
// 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) {
|
// assert openQueue.size() == 0;
|
|
existNodes.clear();
|
openQueue.add(start);
|
existNodes.add(start);
|
|
RetreatNavigateNode firstPointOfTurn = null;
|
List<String> firstPointWaveScopeOfTurn = new ArrayList<>();
|
|
while (openQueue.size() > 0 && null == finialNode) {
|
|
RetreatNavigateNode currentNode = openQueue.poll();
|
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());
|
}
|
|
for (RetreatNavigateNode node : neighborNodes) {
|
|
int weight = 0;
|
|
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);
|
if (!Cools.isEmpty(otherWaveList)) {
|
|
weight += otherWaveList.size();
|
}
|
}
|
|
// have cross turn road
|
if (null != firstPointOfTurn) {
|
if (!firstPointWaveScopeOfTurn.contains(node.getCodeData())) {
|
enableNodes.add(node);
|
}
|
}
|
|
node.setLastDistance(calcNodeCost(currentNode, node));
|
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(1);
|
}
|
|
}
|
|
}
|
|
return finialNode;
|
}
|
|
private int calcNodeWeightVal(List<String> otherWaveList) {
|
int weightVal = 0;
|
|
if (!Cools.isEmpty(otherWaveList)) {
|
|
|
}
|
|
|
return weightVal;
|
}
|
|
// 获取四周节点
|
private ArrayList<RetreatNavigateNode> getNeighborNodes(RetreatNavigateNode currentNode, List<RetreatNavigateNode> existNodes) {
|
|
int x = currentNode.getX();
|
int y = currentNode.getY();
|
|
ArrayList<RetreatNavigateNode> neighbourNodes = new ArrayList<>();
|
|
RetreatNavigateNode rightNode = extendNeighborNodes(currentNode, new RetreatNavigateNode(x, y + 1), existNodes, null, null);
|
if (null != rightNode) {
|
neighbourNodes.add(rightNode);
|
}
|
|
RetreatNavigateNode leftNode = extendNeighborNodes(currentNode, new RetreatNavigateNode(x, y - 1), existNodes, null, null);
|
if (null != leftNode) {
|
neighbourNodes.add(leftNode);
|
}
|
|
RetreatNavigateNode topNode = extendNeighborNodes(currentNode, new RetreatNavigateNode(x - 1, y), existNodes, null, null);
|
if (null != topNode) {
|
neighbourNodes.add(topNode);
|
}
|
|
RetreatNavigateNode bottomNode = extendNeighborNodes(currentNode, new RetreatNavigateNode(x + 1, y), existNodes, null, null);
|
if (null != bottomNode) {
|
neighbourNodes.add(bottomNode);
|
}
|
|
return neighbourNodes;
|
}
|
|
private RetreatNavigateNode extendNeighborNodes(RetreatNavigateNode currentNode, RetreatNavigateNode extendNode, List<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);
|
|
} else {
|
if (this.isExist(nextNode, existNodes)) {
|
return null;
|
}
|
|
// 节点是否可用
|
if (mapMatrix[x][y] != MapNodeType.ENABLE.val) {
|
return null;
|
}
|
|
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
|
String currentNodeCodeData = codeMatrix[currentNode.getX()][currentNode.getY()];
|
String nextNodeCodeData = codeMatrix[nextNode.getX()][nextNode.getY()];
|
nextNode.setCodeData(nextNodeCodeData);
|
|
// 判断通过性
|
Route route = routeService.findByCodeOfBoth(
|
codeService.selectByData(currentNodeCodeData).getId(),
|
codeService.selectByData(nextNodeCodeData).getId()
|
);
|
if (null == route) {
|
return null;
|
}
|
|
}
|
|
return nextNode;
|
}
|
|
private boolean isExist(RetreatNavigateNode node, List<RetreatNavigateNode> existNodes) {
|
for (RetreatNavigateNode existNode : existNodes) {
|
if (this.isSame(node, existNode)) {
|
return true;
|
}
|
}
|
return false;
|
}
|
|
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());
|
}
|
|
}
|
