package com.zy.acs.manager.core.service;
|
|
import com.alibaba.fastjson.JSONObject;
|
import com.zy.acs.common.enums.AgvDirectionType;
|
import com.zy.acs.framework.common.Cools;
|
import com.zy.acs.manager.common.utils.MapDataUtils;
|
import com.zy.acs.manager.core.service.astart.*;
|
import com.zy.acs.manager.core.service.astart.domain.DynamicNode;
|
import com.zy.acs.manager.core.service.floyd.FloydNavigateService;
|
import com.zy.acs.manager.manager.entity.Code;
|
import com.zy.acs.manager.manager.entity.Loc;
|
import com.zy.acs.manager.manager.service.ActionService;
|
import com.zy.acs.manager.manager.service.CodeService;
|
import com.zy.acs.manager.system.service.ConfigService;
|
import lombok.extern.slf4j.Slf4j;
|
import org.apache.commons.lang.time.StopWatch;
|
import org.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.stereotype.Component;
|
|
import java.util.*;
|
import java.util.stream.Collectors;
|
|
/**
|
* Created by vincent on 2023/6/14
|
*/
|
@Slf4j
|
@Component("mapService")
|
public class MapService {
|
|
@Autowired
|
private CodeService codeService;
|
@Autowired
|
private FloydNavigateService floydNavigateService;
|
@Autowired
|
private MapDataDispatcher mapDataDispatcher;
|
@Autowired
|
private AStarNavigateService aStarNavigateService;
|
@Autowired
|
private ConfigService configService;
|
@Autowired
|
private ActionService actionService;
|
|
public List<String> checkoutPath(String agvNo, Code startCode, Code endCode, Boolean lock) {
|
return this.checkoutPath(agvNo, startCode, endCode, lock, null, null);
|
}
|
|
/**
|
* 寻址 ===>> A*
|
*/
|
public synchronized List<String> checkoutPath(String agvNo, Code startCode, Code endCode
|
, Boolean lock, List<String> whiteList, List<String> blackList) {
|
|
int[] startMapIdx = mapDataDispatcher.getCodeMatrixIdx(null, startCode.getData());
|
int[] endMapIdx = mapDataDispatcher.getCodeMatrixIdx(null, endCode.getData());
|
|
NavigateNode startNode = new NavigateNode(startMapIdx[0], startMapIdx[1], startCode.getData());
|
NavigateNode endNode = new NavigateNode(endMapIdx[0], endMapIdx[1], endCode.getData());
|
|
NavigateNode finishNode = aStarNavigateService.execute(agvNo, startNode, endNode, lock, whiteList, blackList);
|
|
if (null == finishNode) {
|
log.warn("{} 号AGV检索[{}] ===>> [{}]路径失败......", agvNo, startCode.getData(), endCode.getData());
|
return new ArrayList<>();
|
}
|
|
ArrayList<NavigateNode> navigateNodes = new ArrayList<>();
|
|
// 渲染
|
NavigateNode parentNode = null;//当前循环上一节点，用于拐点计算
|
while (finishNode != null) {
|
navigateNodes.add(finishNode);
|
|
parentNode = finishNode;
|
finishNode = finishNode.getParent();
|
}
|
|
Collections.reverse(navigateNodes);
|
|
//将每个节点里面的fatherNode至为null(方便后续计算时父节点过多导致显示的节点太多)
|
for (NavigateNode navigateNode : navigateNodes) {
|
//父节点设置为null，不影响计算结果，不影响后续操作。
|
//此操作仅为后续排查处理提供视觉方便。
|
navigateNode.setParent(null);
|
}
|
|
return navigateNodes.stream().map(NavigateNode::getCodeData).collect(Collectors.toList());
|
}
|
|
/**
|
* 寻址 ===>> Floyd
|
*/
|
public synchronized List<String> validFeasibility(Code startCode, Code endCode) {
|
|
int startIdx = floydNavigateService.codeIdx(startCode.getId());
|
int endIdx = floydNavigateService.codeIdx(endCode.getId());
|
|
Double distance;
|
try {
|
distance = floydNavigateService.getFloydMatrix()[startIdx][endIdx];
|
} catch (ArrayIndexOutOfBoundsException e) {
|
floydNavigateService.generateMatrix();
|
startIdx = floydNavigateService.codeIdx(startCode.getId());
|
endIdx = floydNavigateService.codeIdx(endCode.getId());
|
distance = floydNavigateService.getFloydMatrix()[startIdx][endIdx];
|
}
|
|
if (distance.equals(FloydNavigateService.INF)) {
|
return null;
|
}
|
|
return floydNavigateService.calculatePath(startIdx, endIdx).stream().map(path -> {
|
Long codeId = floydNavigateService.getCodeId(path);
|
Code code = codeService.getById(codeId);
|
return code.getData();
|
}).collect(Collectors.toList());
|
}
|
|
|
// 角度计算
|
public Double calculateDirection(Code startCode, Code endCode) {
|
Double x0 = startCode.getX();
|
Double y0 = startCode.getY();
|
|
Double x1 = endCode.getX();
|
Double y1 = endCode.getY();
|
|
double deltaX = x1 - x0;
|
double deltaY = y1 - y0;
|
double angle = Math.atan2(deltaX, deltaY);
|
int offsetAngle = configService.getVal("mapXoffset", Integer.class);
|
angle = angle + offsetAngle;
|
angle = Math.toDegrees(angle);
|
angle = (angle + 360) % 360; // 将角度转换为正值
|
|
return angle;
|
}
|
|
// 坐标货架阈值 todo:luxiaotao
|
public AgvDirectionType calculateAgvWorkDirection(JSONObject storeDirection, Loc loc, Code code) {
|
storeDirection.getString("key");
|
Integer compDirect = loc.getCompDirect();
|
AgvDirectionType agvDirectionType = null;
|
if (compDirect == 0) {
|
agvDirectionType = AgvDirectionType.RIGHT;
|
}
|
if (compDirect == 1) {
|
agvDirectionType = AgvDirectionType.LEFT;
|
}
|
return agvDirectionType;
|
}
|
|
|
public double calculateDistance(double x1, double y1, double x2, double y2) {
|
double deltaX = x2 - x1;
|
double deltaY = y2 - y1;
|
return Math.sqrt(deltaX * deltaX + deltaY * deltaY);
|
}
|
|
public DirectionType calcDirectionType(int[] originIdx, int[] oppositeIdx) {
|
if (oppositeIdx[0] < originIdx[0] && oppositeIdx[1] == originIdx[1]) {
|
return DirectionType.TOP;
|
}
|
if (oppositeIdx[0] > originIdx[0] && oppositeIdx[1] == originIdx[1]) {
|
return DirectionType.BOTTOM;
|
}
|
if (oppositeIdx[0] == originIdx[0] && oppositeIdx[1] > originIdx[1]) {
|
return DirectionType.RIGHT;
|
}
|
if (oppositeIdx[0] == originIdx[0] && oppositeIdx[1] < originIdx[1]) {
|
return DirectionType.LEFT;
|
}
|
return DirectionType.NONE;
|
}
|
|
public void lockPath(Integer lev, List<String> pathList, String agvNo) {
|
mapDataDispatcher.modifyDynamicMatrix(lev, pathList, agvNo);
|
}
|
|
public synchronized void unlockPath(String agvNo, List<String> nodeList) {
|
try {
|
StopWatch stopWatch = new StopWatch();
|
stopWatch.start();
|
|
if (Cools.isEmpty(agvNo, nodeList)) {
|
return;
|
}
|
|
List<String> pathList = actionService.getPathListByAgv(agvNo).stream().distinct().collect(Collectors.toList());
|
|
List<String> inTrace = new ArrayList<>();
|
|
Set<String> outsideTrace = new HashSet<>();
|
|
if (Cools.isEmpty(pathList)) {
|
outsideTrace.addAll(nodeList);
|
} else {
|
|
int size = nodeList.size();
|
for (String code : pathList) {
|
|
|
Iterator<String> iterator = nodeList.iterator();
|
while (iterator.hasNext()) {
|
|
String next = iterator.next();
|
|
if (next.equals(code)) {
|
if (!inTrace.contains(next)) {
|
inTrace.add(next);
|
}
|
|
iterator.remove();
|
}
|
}
|
|
}
|
|
|
assert inTrace.size() + nodeList.size() <= size;
|
|
if (!Cools.isEmpty(nodeList)) {
|
outsideTrace.addAll(nodeList);
|
}
|
|
if (!Cools.isEmpty(inTrace)) {
|
|
String last = inTrace.get(inTrace.size() - 1);
|
|
int idx = pathList.indexOf(last);
|
inTrace = pathList.subList(0, idx + 1);
|
}
|
|
|
}
|
|
List<String> resetCodeList = new ArrayList<>();
|
|
Integer lev = null;
|
|
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
|
|
for (String code : inTrace) {
|
int[] node = mapDataDispatcher.getCodeMatrixIdx(lev, code);
|
DynamicNode dynamicNode = dynamicMatrix[node[0]][node[1]];
|
|
if (dynamicNode.getVehicle().equals(agvNo)) {
|
resetCodeList.add(code);
|
}
|
}
|
|
for (String code : outsideTrace) {
|
int[] node = mapDataDispatcher.getCodeMatrixIdx(lev, code);
|
DynamicNode dynamicNode = dynamicMatrix[node[0]][node[1]];
|
|
if (dynamicNode.getVehicle().equals(agvNo)) {
|
resetCodeList.add(code);
|
}
|
}
|
|
if (!Cools.isEmpty(resetCodeList)) {
|
|
mapDataDispatcher.clearDynamicMatrixByCodeList(lev, resetCodeList);
|
}
|
|
stopWatch.stop();
|
if (stopWatch.getTime() > 50) {
|
log.info("解锁路径函数花费时间为：{}毫秒......", stopWatch.getTime());
|
}
|
|
} catch (Exception e) {
|
log.error("TrafficService.unlockPath", e);
|
}
|
|
}
|
|
public synchronized void unlockPath(String agvNo, String codeData) {
|
try {
|
StopWatch stopWatch = new StopWatch();
|
stopWatch.start();
|
|
if (Cools.isEmpty(agvNo, codeData)) {
|
return;
|
}
|
|
Integer lev = null;
|
|
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
|
int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, codeData);
|
|
|
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
|
|
DynamicNode dynamicNode = dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
|
|
|
Integer serial = dynamicNode.getSerial();
|
|
List<String> resetCodeList = new ArrayList<>();
|
|
for (int i = 0; i < dynamicMatrix.length; i++) {
|
for (int j = 0; j < dynamicMatrix[i].length; j++) {
|
DynamicNode node = dynamicMatrix[i][j];
|
if (node.getVehicle().equals(agvNo) && node.getSerial() < serial) {
|
resetCodeList.add(codeMatrix[i][j]);
|
}
|
}
|
}
|
|
if (!Cools.isEmpty(resetCodeList)) {
|
|
mapDataDispatcher.clearDynamicMatrixByCodeList(lev, resetCodeList);
|
}
|
|
stopWatch.stop();
|
if (stopWatch.getTime() > 50) {
|
log.info("解锁路径函数花费时间为：{}毫秒......", stopWatch.getTime());
|
}
|
|
} catch (Exception e) {
|
log.error("TrafficService.unlockPath", e);
|
}
|
|
}
|
|
public List<String> getWaveScopeByCodeList(Integer lev, List<String> codeList, Double radiusLen) {
|
if (Cools.isEmpty(codeList)) {
|
return new ArrayList<>();
|
}
|
if (Cools.isEmpty(radiusLen) || radiusLen == 0D) {
|
return codeList;
|
}
|
radiusLen = Math.abs(radiusLen);
|
|
List<NavigateNode> nodeList = new ArrayList<>();
|
for (String code : codeList) {
|
|
nodeList.addAll(this.getWaveScopeByCode(lev, code, radiusLen));
|
}
|
|
return nodeList.stream().map(NavigateNode::getCodeData).distinct().collect(Collectors.toList());
|
}
|
|
public List<NavigateNode> getWaveScopeByCode(Integer lev, String code, Double radiusLen) {
|
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
|
|
int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code);
|
NavigateNode originNode = new NavigateNode(codeMatrixIdx[0], codeMatrixIdx[1], code);
|
|
List<NavigateNode> includeList = new ArrayList<>();
|
List<NavigateNode> existNodes = new ArrayList<>();
|
|
includeList.add(originNode);
|
existNodes.add(originNode);
|
|
this.spreadWaveNode(originNode, originNode, codeMatrix, radiusLen, includeList, existNodes);
|
|
return includeList;
|
}
|
|
public void spreadWaveNode(NavigateNode originNode, NavigateNode currNode
|
, String[][] codeMatrix, Double radiusLen
|
, List<NavigateNode> includeList, List<NavigateNode> existNodes) {
|
int x = currNode.getX();
|
int y = currNode.getY();
|
|
this.extendNeighborNodes(originNode, new NavigateNode(x, y + 1), codeMatrix, radiusLen, includeList, existNodes);
|
this.extendNeighborNodes(originNode, new NavigateNode(x, y - 1), codeMatrix, radiusLen, includeList, existNodes);
|
this.extendNeighborNodes(originNode, new NavigateNode(x - 1, y), codeMatrix, radiusLen, includeList, existNodes);
|
this.extendNeighborNodes(originNode, new NavigateNode(x + 1, y), codeMatrix, radiusLen, includeList, existNodes);
|
}
|
|
public void extendNeighborNodes(NavigateNode originNode, NavigateNode nextNode
|
, String[][] codeMatrix, Double radiusLen
|
, List<NavigateNode> includeList, List<NavigateNode> existNodes) {
|
|
int x = nextNode.getX();
|
int y = nextNode.getY();
|
|
if (x < 0 || x >= codeMatrix.length
|
|| y < 0 || y >= codeMatrix[0].length) {
|
return;
|
}
|
|
if (this.isExist(nextNode, existNodes)) {
|
return;
|
}
|
|
existNodes.add(nextNode);
|
|
String nextNodeCodeData = codeMatrix[x][y];
|
|
if (nextNodeCodeData.equals(CodeNodeType.NONE.val)) {
|
|
this.spreadWaveNode(originNode, nextNode, codeMatrix, radiusLen, includeList, existNodes);
|
|
} else {
|
|
Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV;
|
String[][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
|
|
// Code o1 = codeService.selectByData(originNode.getCodeData());
|
// Code o2 = codeService.selectByData(nextNodeCodeData);
|
List<Double> o1Cda = MapDataUtils.parseCdaNode(cdaMatrix[originNode.getX()][originNode.getY()]);
|
List<Double> o2Cda = MapDataUtils.parseCdaNode(cdaMatrix[nextNode.getX()][nextNode.getY()]);
|
|
// if (Math.pow(o1.getX() - o2.getX(), 2) + Math.pow(o1.getY() - o2.getY(), 2) <= Math.pow(radiusLen, 2)) {
|
if (Math.pow(o1Cda.get(0) - o2Cda.get(0), 2) + Math.pow(o1Cda.get(1) - o2Cda.get(1), 2) <= Math.pow(radiusLen, 2)) {
|
nextNode.setCodeData(nextNodeCodeData);
|
includeList.add(nextNode);
|
|
this.spreadWaveNode(originNode, nextNode, codeMatrix, radiusLen, includeList, existNodes);
|
|
}
|
}
|
}
|
|
private boolean isExist(NavigateNode node, List<NavigateNode> existNodes) {
|
for (NavigateNode existNode : existNodes) {
|
if (this.isSame(node, existNode)) {
|
return true;
|
}
|
}
|
return false;
|
}
|
|
private boolean isSame(NavigateNode o1, NavigateNode o2) {
|
if (Cools.isEmpty(o1, o2)) {
|
return false;
|
}
|
return o1.getX() == o2.getX() && o1.getY() == o2.getY();
|
}
|
|
public Boolean isTurnCorner(String codeData) {
|
if (Cools.isEmpty(codeData)) {
|
return false;
|
}
|
|
int[][] turnMatrix = mapDataDispatcher.getTurnMatrix(null);
|
|
int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(null, codeData);
|
int turnMatrixNode = turnMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
|
|
return turnMatrixNode == TurnNodeType.TURN.val;
|
}
|
|
}
|
