package com.zy.acs.manager.core.service;
|
|
import com.zy.acs.common.enums.AgvDirectionType;
|
import com.zy.acs.framework.common.Cools;
|
import com.zy.acs.manager.core.constant.MapDataConstant;
|
import com.zy.acs.manager.core.domain.SortCodeDto;
|
import com.zy.acs.manager.core.domain.UnlockPathTask;
|
import com.zy.acs.manager.core.service.astart.*;
|
import com.zy.acs.manager.core.service.astart.domain.AStarNavigateNode;
|
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.entity.Segment;
|
import com.zy.acs.manager.manager.entity.Sta;
|
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.springframework.beans.factory.annotation.Autowired;
|
import org.springframework.beans.factory.annotation.Value;
|
import org.springframework.stereotype.Component;
|
|
import java.util.*;
|
import java.util.concurrent.ConcurrentLinkedQueue;
|
import java.util.concurrent.LinkedBlockingQueue;
|
import java.util.concurrent.TimeUnit;
|
import java.util.stream.Collectors;
|
import java.util.stream.IntStream;
|
|
/**
|
* Created by vincent on 2023/6/14
|
*/
|
@Slf4j
|
@Component("mapService")
|
public class MapService {
|
|
private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};
|
|
@Value("${floyd.enable}")
|
private Boolean floydEnable;
|
@Autowired
|
private CodeService codeService;
|
@Autowired
|
private FloydNavigateService floydNavigateService;
|
@Autowired
|
private MapDataDispatcher mapDataDispatcher;
|
@Autowired
|
private AStarNavigateService aStarNavigateService;
|
@Autowired
|
private ConfigService configService;
|
@Autowired
|
private ActionService actionService;
|
@Autowired
|
private LinkedBlockingQueue<UnlockPathTask> unlockTaskQueue;
|
|
/**
|
* 寻址 ===>> A Star
|
*/
|
public synchronized List<String> checkoutPath(String agvNo, Code startCode, Code endCode
|
, Boolean lock, List<String> blackList, Segment segment) {
|
|
int[] startMapIdx = mapDataDispatcher.getCodeMatrixIdx(null, startCode.getData());
|
int[] endMapIdx = mapDataDispatcher.getCodeMatrixIdx(null, endCode.getData());
|
|
AStarNavigateNode startNode = new AStarNavigateNode(startMapIdx[0], startMapIdx[1], startCode.getData());
|
AStarNavigateNode endNode = new AStarNavigateNode(endMapIdx[0], endMapIdx[1], endCode.getData());
|
|
// long startTime = System.currentTimeMillis();
|
AStarNavigateNode finishNode = aStarNavigateService.execute(agvNo, startNode, endNode, lock, blackList, segment);
|
// System.out.println("A Star Spend time :" + (System.currentTimeMillis() - startTime));
|
if (null == finishNode) {
|
return new ArrayList<>();
|
}
|
|
ArrayList<AStarNavigateNode> navigateNodes = new ArrayList<>();
|
|
while (finishNode != null) {
|
navigateNodes.add(finishNode);
|
finishNode = finishNode.getParent();
|
}
|
|
Collections.reverse(navigateNodes);
|
|
List<String> navigatePath = navigateNodes.stream().map(AStarNavigateNode::getCodeData).collect(Collectors.toList());
|
|
// max count of steps
|
if (navigatePath.size() > MapDataConstant.MAX_STEPS_SINGLE) {
|
navigatePath = navigatePath.subList(0, MapDataConstant.MAX_STEPS_SINGLE);
|
}
|
|
return navigatePath;
|
}
|
|
/**
|
* 寻址 ===>> Floyd
|
*/
|
public synchronized List<String> validFeasibility(Code startCode, Code endCode) {
|
if (!floydEnable) {
|
return Arrays.asList("00000001", "00000002", "00000003", "00000004", "00000005", "00000006");
|
}
|
|
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.getCacheById(codeId);
|
return code.getData();
|
}).collect(Collectors.toList());
|
}
|
|
public static Double mapToNearest(Double angle) {
|
if (null == angle) {
|
return null;
|
}
|
angle = (angle + 360) % 360;
|
double remainder = angle % 45;
|
if (remainder < 22.5) {
|
return angle - remainder;
|
} else {
|
return angle + (45 - remainder);
|
}
|
}
|
|
// 角度计算
|
public Double calculateDirection(Code startCode, Code endCode, int angleOffsetVal) {
|
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);
|
angle = Math.toDegrees(angle) + angleOffsetVal;
|
angle = (angle + 360) % 360; // 将角度转换为正值
|
|
return angle;
|
}
|
|
// 坐标货架阈值 todo:luxiaotao
|
public AgvDirectionType calculateAgvWorkDirectionByShelf(Loc loc, Code code) {
|
Integer compDirect = loc.getCompDirect();
|
AgvDirectionType agvDirectionType = null;
|
if (compDirect == 0) {
|
agvDirectionType = AgvDirectionType.RIGHT;
|
}
|
if (compDirect == 1) {
|
agvDirectionType = AgvDirectionType.LEFT;
|
}
|
return agvDirectionType;
|
}
|
|
public Double getStaAngle(Sta sta, Double workDirection) {
|
if (null == sta) {
|
return null;
|
}
|
if (Cools.isEmpty(sta.getAngle())) {
|
return workDirection;
|
}
|
return Double.parseDouble(sta.getAngle());
|
}
|
|
public Double calculateAgvWorkDirectionByStation(Double staWorkDirection, Double lastDirection) {
|
return Math.abs(staWorkDirection - lastDirection) + 90.0D;
|
}
|
|
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) {
|
List<int[]> codeMatrixIdxList = mapDataDispatcher.getCodeMatrixIdxList(lev, pathList);
|
mapDataDispatcher.modifyDynamicMatrix(lev, codeMatrixIdxList, agvNo);
|
}
|
|
public void unlockPath(String agvNo, String codeData) {
|
if (Cools.isEmpty(agvNo, codeData)) {
|
return;
|
}
|
try {
|
unlockTaskQueue.offer(new UnlockPathTask(agvNo, codeData), 5, TimeUnit.SECONDS);
|
|
// 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]];
|
//
|
//
|
// int serial = dynamicNode.getSerial();
|
//
|
// List<int[]> resetCodeIdxList = new ArrayList<>();
|
//
|
// for (int i = 0; i < dynamicMatrix.length; i++) {
|
// for (int j = 0; j < dynamicMatrix[i].length; j++) {
|
//
|
//// if (i == codeMatrixIdx[0] && j == codeMatrixIdx[1]) { continue; }
|
//
|
// DynamicNode node = dynamicMatrix[i][j];
|
// if (node.getVehicle().equals(agvNo)) {
|
// if (node.getSerial() < serial) {
|
// resetCodeIdxList.add(new int[] {i, j});
|
// }
|
// }
|
// }
|
// }
|
//
|
// if (!Cools.isEmpty(resetCodeIdxList)) {
|
//
|
// mapDataDispatcher.clearDynamicMatrixByCodeList(lev, resetCodeIdxList);
|
// }
|
|
} catch (InterruptedException e) {
|
log.error("unlockTaskQueue", 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());
|
}
|
|
// v1 BFS ------------------------------------------------------------------------------
|
public List<NavigateNode> getWaveScopeByCode(Integer lev, String code, Double radiusLen) {
|
double radiusLenSquared = Math.pow(radiusLen, 2);
|
|
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
|
Double[][][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
|
|
int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code);
|
NavigateNode originNode = new NavigateNode(codeMatrixIdx[0], codeMatrixIdx[1], code);
|
|
List<NavigateNode> includeList = new ArrayList<>();
|
Set<NavigateNode> existNodes = new HashSet<>();
|
|
includeList.add(originNode);
|
existNodes.add(originNode);
|
|
this.spreadWaveNode(originNode, originNode, codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
|
|
return includeList;
|
}
|
|
public void spreadWaveNode(NavigateNode originNode, NavigateNode currNode
|
, String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLenSquared
|
, List<NavigateNode> includeList, Set<NavigateNode> existNodes) {
|
int x = currNode.getX();
|
int y = currNode.getY();
|
|
this.extendNeighborNodes(originNode, new NavigateNode(x, y + 1), codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
|
this.extendNeighborNodes(originNode, new NavigateNode(x, y - 1), codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
|
this.extendNeighborNodes(originNode, new NavigateNode(x - 1, y), codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
|
this.extendNeighborNodes(originNode, new NavigateNode(x + 1, y), codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
|
}
|
|
public void extendNeighborNodes(NavigateNode originNode, NavigateNode nextNode
|
, String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLenSquared
|
, List<NavigateNode> includeList, Set<NavigateNode> existNodes) {
|
|
int x = nextNode.getX();
|
int y = nextNode.getY();
|
|
if (x < 0 || x >= codeMatrix.length
|
|| y < 0 || y >= codeMatrix[0].length) {
|
return;
|
}
|
|
if (existNodes.contains(nextNode)) {
|
return;
|
}
|
|
existNodes.add(nextNode);
|
|
Double[] o1Cda = cdaMatrix[originNode.getX()][originNode.getY()];
|
Double[] o2Cda = cdaMatrix[x][y];
|
if (Math.pow(o1Cda[0] - o2Cda[0], 2) + Math.pow(o1Cda[1] - o2Cda[1], 2) <= radiusLenSquared) {
|
nextNode.setCodeData(codeMatrix[x][y]);
|
|
if (!nextNode.getCodeData().equals(CodeNodeType.NONE.val)) {
|
includeList.add(nextNode);
|
}
|
|
this.spreadWaveNode(originNode, nextNode, codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
|
}
|
|
}
|
|
// v2 BFS ------------------------------------------------------------------------------
|
// public List<NavigateNode> getWaveScopeByCode0(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<>();
|
// Set<NavigateNode> visited = new HashSet<>(); // Track visited nodes to avoid re-processing
|
// Queue<NavigateNode> queue = new LinkedList<>();
|
//
|
// includeList.add(originNode);
|
// visited.add(originNode);
|
// queue.offer(originNode);
|
//
|
// while (!queue.isEmpty()) {
|
// NavigateNode currNode = queue.poll();
|
// this.spreadWaveNode0(originNode, currNode, codeMatrix, radiusLen, includeList, visited, queue);
|
// }
|
//
|
// return includeList;
|
// }
|
//
|
// public void spreadWaveNode0(NavigateNode originNode, NavigateNode currNode,
|
// String[][] codeMatrix, Double radiusLen,
|
// List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) {
|
//
|
// int x = currNode.getX();
|
// int y = currNode.getY();
|
//
|
// // Expand neighbors in all four directions (up, down, left, right)
|
// this.extendNeighborNodes0(originNode, new NavigateNode(x, y + 1), codeMatrix, radiusLen, includeList, visited, queue);
|
// this.extendNeighborNodes0(originNode, new NavigateNode(x, y - 1), codeMatrix, radiusLen, includeList, visited, queue);
|
// this.extendNeighborNodes0(originNode, new NavigateNode(x - 1, y), codeMatrix, radiusLen, includeList, visited, queue);
|
// this.extendNeighborNodes0(originNode, new NavigateNode(x + 1, y), codeMatrix, radiusLen, includeList, visited, queue);
|
// }
|
//
|
// public void extendNeighborNodes0(NavigateNode originNode, NavigateNode nextNode,
|
// String[][] codeMatrix, Double radiusLen,
|
// List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) {
|
// int x = nextNode.getX();
|
// int y = nextNode.getY();
|
//
|
// // Check if the node is out of bounds
|
// if (x < 0 || x >= codeMatrix.length || y < 0 || y >= codeMatrix[0].length) {
|
// return;
|
// }
|
//
|
// // If the node has already been visited, skip it
|
// if (visited.contains(nextNode)) {
|
// return;
|
// }
|
//
|
// visited.add(nextNode);
|
//
|
// String nextNodeCodeData = codeMatrix[x][y];
|
//
|
// // If it's a NONE node, we still need to check its surroundings
|
// if (nextNodeCodeData.equals(CodeNodeType.NONE.val)) {
|
// this.spreadWaveNode0(originNode, nextNode, codeMatrix, radiusLen, includeList, visited, queue);
|
// } else {
|
// Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV;
|
// String[][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
|
//
|
// // Check if the distance between nodes is within the radius length
|
// List<Double> o1Cda = MapDataUtils.parseCdaNode(cdaMatrix[originNode.getX()][originNode.getY()]);
|
// List<Double> o2Cda = MapDataUtils.parseCdaNode(cdaMatrix[nextNode.getX()][nextNode.getY()]);
|
//
|
// // Calculate Euclidean distance between the nodes
|
// 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);
|
//
|
// // Add the node to the queue to expand its neighbors
|
// queue.offer(nextNode);
|
// }
|
// }
|
// }
|
|
|
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;
|
}
|
|
/**
|
* That vehicle is walking if the dynamic node count > 1
|
*/
|
@SuppressWarnings("all")
|
public boolean isWalkingByVehicle(Integer lev, String vehicle) {
|
if (Cools.isEmpty(vehicle)) {
|
return false;
|
}
|
|
lev = Optional.ofNullable(lev).orElse(MapDataDispatcher.MAP_DEFAULT_LEV);
|
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
|
|
int count = 0;
|
for (int i = 0; i < dynamicMatrix.length; i++) {
|
for (int j = 0; j < dynamicMatrix[i].length; j++) {
|
if (vehicle.equals(dynamicMatrix[i][j].getVehicle())) {
|
count++;
|
if (count > 1) {
|
return true;
|
}
|
}
|
}
|
}
|
|
return false;
|
}
|
|
public List<String> queryCodeListFromDynamicNode(Integer lev, String nodeType) {
|
if (Cools.isEmpty(nodeType)) {
|
return new ArrayList<>();
|
}
|
lev = Optional.ofNullable(lev).orElse(MapDataDispatcher.MAP_DEFAULT_LEV);
|
|
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
|
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
|
|
// concurrent
|
ConcurrentLinkedQueue<SortCodeDto> codeList = new ConcurrentLinkedQueue<>();
|
IntStream.range(0, codeMatrix.length).parallel().forEach(i -> {
|
for (int j = 0; j < codeMatrix[i].length; j++) {
|
DynamicNode dynamicNode = dynamicMatrix[i][j];
|
if (nodeType.equals(dynamicNode.getVehicle())) {
|
codeList.add(new SortCodeDto(codeMatrix[i][j], dynamicNode.getSerial()));
|
}
|
}
|
});
|
|
// synchronize
|
// List<SortCodeDto> codeList = new ArrayList<>();
|
// for (int i = 0; i < codeMatrix.length; i++) {
|
// for (int j = 0; j < codeMatrix[i].length; j++) {
|
// DynamicNode dynamicNode = dynamicMatrix[i][j];
|
// if (nodeType.equals(dynamicNode.getVehicle())) {
|
// codeList.add(new SortCodeDto(codeMatrix[i][j], dynamicNode.getSerial()));
|
// }
|
// }
|
// }
|
|
return codeList.stream()
|
.sorted(Comparator.comparingInt(SortCodeDto::getSerial))
|
.map(SortCodeDto::getCode)
|
.collect(Collectors.toList());
|
}
|
|
public Map<String, List<String>> queryCodeListFromDynamicNode(Integer lev, Set<String> vehicles) {
|
if (Cools.isEmpty(vehicles)) {
|
return new HashMap<>();
|
}
|
lev = Optional.ofNullable(lev).orElse(MapDataDispatcher.MAP_DEFAULT_LEV);
|
|
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
|
String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
|
|
Map<String, List<SortCodeDto>> map = new HashMap<>(vehicles.size());
|
|
for (int i = 0; i < codeMatrix.length; i++) {
|
for (int j = 0; j < codeMatrix[i].length; j++) {
|
DynamicNode dynamicNode = dynamicMatrix[i][j];
|
String vehicle = dynamicNode.getVehicle();
|
|
if (vehicles.contains(vehicle)) {
|
int serial = dynamicNode.getSerial();
|
map.computeIfAbsent(vehicle, k -> new ArrayList<>()).add(new SortCodeDto(codeMatrix[i][j], serial));
|
}
|
}
|
}
|
|
Map<String, List<String>> result = new HashMap<>(vehicles.size());
|
for (Map.Entry<String, List<SortCodeDto>> entry : map.entrySet()) {
|
List<String> codeDataList = entry.getValue().stream()
|
.sorted(Comparator.comparingInt(SortCodeDto::getSerial))
|
.map(SortCodeDto::getCode)
|
.collect(Collectors.toList());
|
result.put(entry.getKey(), codeDataList);
|
}
|
|
return result;
|
}
|
|
}
|
