From 80f5003a640db7795d69c5e3a73caa685c289b80 Mon Sep 17 00:00:00 2001
From: luxiaotao1123 <t1341870251@gmail.com>
Date: 星期三, 14 一月 2026 16:27:38 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java | 222 ++++++++++++++++++++++++++++++++++++++++++++++++------
1 files changed, 195 insertions(+), 27 deletions(-)
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
index 2d71496..d792dd8 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
@@ -1,10 +1,13 @@
package com.zy.acs.manager.core.service;
+import com.alibaba.fastjson.JSON;
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.DirectionDto;
import com.zy.acs.manager.core.domain.SortCodeDto;
import com.zy.acs.manager.core.domain.UnlockPathTask;
+import com.zy.acs.manager.core.domain.type.CodeDirectionType;
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;
@@ -12,6 +15,8 @@
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.enums.CodeSpinType;
import com.zy.acs.manager.manager.service.ActionService;
import com.zy.acs.manager.manager.service.CodeService;
import com.zy.acs.manager.system.service.ConfigService;
@@ -34,6 +39,7 @@
@Component("mapService")
public class MapService {
+ private static final double EPS = 1e-7;
private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};
@Value("${floyd.enable}")
@@ -123,6 +129,18 @@
}).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) {
@@ -134,7 +152,7 @@
double deltaX = x1 - x0;
double deltaY = y1 - y0;
- double angle = -Math.atan2(deltaX, deltaY);
+ double angle = Math.atan2(deltaX, deltaY);
angle = Math.toDegrees(angle) + angleOffsetVal;
angle = (angle + 360) % 360; // 灏嗚搴﹁浆鎹负姝ｅ��
@@ -142,7 +160,7 @@
}
// 鍧愭爣璐ф灦闃堝�� todo:luxiaotao
- public AgvDirectionType calculateAgvWorkDirection(Loc loc, Code code) {
+ public AgvDirectionType calculateAgvWorkDirectionByShelf(Loc loc, Code code) {
Integer compDirect = loc.getCompDirect();
AgvDirectionType agvDirectionType = null;
if (compDirect == 0) {
@@ -154,27 +172,24 @@
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) {
@@ -183,8 +198,46 @@
}
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);
}
@@ -210,6 +263,8 @@
// 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);
@@ -222,25 +277,25 @@
includeList.add(originNode);
existNodes.add(originNode);
- this.spreadWaveNode(originNode, originNode, codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
+ this.spreadWaveNode(originNode, originNode, codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
return includeList;
}
public void spreadWaveNode(NavigateNode originNode, NavigateNode currNode
- , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLen
+ , 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, radiusLen, includeList, existNodes);
- this.extendNeighborNodes(originNode, new NavigateNode(x, y - 1), codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
- this.extendNeighborNodes(originNode, new NavigateNode(x - 1, y), codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
- this.extendNeighborNodes(originNode, new NavigateNode(x + 1, y), codeMatrix, cdaMatrix, radiusLen, includeList, existNodes);
+ 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 radiusLen
+ , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLenSquared
, List<NavigateNode> includeList, Set<NavigateNode> existNodes) {
int x = nextNode.getX();
@@ -259,14 +314,14 @@
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) <= Math.pow(radiusLen, 2)) {
+ 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, radiusLen, includeList, existNodes);
+ this.spreadWaveNode(originNode, nextNode, codeMatrix, cdaMatrix, radiusLenSquared, includeList, existNodes);
}
}
@@ -364,6 +419,119 @@
return turnMatrixNode == TurnNodeType.TURN.val;
}
+ public CodeSpinType spinDirection(Code code) {
+ if (Cools.isEmpty(code)) {
+ return CodeSpinType.NA;
+ }
+ return CodeSpinType.of(code.getSpin());
+ }
+
+ public static void main(String[] args) {
+ CodeSpinType codeSpinType = calcSpinDirection(null, 260.0, 10.0);
+ List<DirectionDto> directionDtoList = DirectionDto.initCodeDirections();
+ System.out.println(codeSpinType.toString());
+ }
+
+ public static CodeSpinType calcSpinDirection(Code code, Double currDir, Double nextDir) {
+ if (Cools.isEmpty(code, currDir, nextDir)) {
+ return CodeSpinType.NA;
+ }
+ List<Double> disableAngleList = new ArrayList<>();
+ List<DirectionDto> directionDtoList = DirectionDto.initCodeDirections();
+ if (code.getData().contains("57")) {
+ directionDtoList = DirectionDto.initCodeDirections0();
+ }
+ for (DirectionDto dto : directionDtoList) {
+ if (null != dto.getEnabled() && !dto.getEnabled()) {
+ disableAngleList.add(dto.getAngle());
+ }
+ }
+
+ if (Cools.isEmpty(disableAngleList)) {
+ return CodeSpinType.NA;
+ }
+
+ double curr = norm360(currDir);
+ double next = norm360(nextDir);
+
+ // 濡傛灉鍑犱箮鐩哥瓑锛岃涓轰笉闇�瑕佹棆杞�
+ if (almostEqual(curr, next)) {
+ return CodeSpinType.NA;
+ }
+
+ double cwDelta = cwDelta(curr, next);
+ double ccwDelta = 360.0 - cwDelta;
+
+ boolean cwBlocked = false;
+ boolean ccwBlocked = false;
+
+ for (Double disableAngle : disableAngleList) {
+ if (disableAngle == null) continue;
+ double disable = norm360(disableAngle);
+
+ if (isOnArcCW(curr, next, disable)) {
+ cwBlocked = true;
+ }
+ if (isOnArcCCW(curr, next, disable)) {
+ ccwBlocked = true;
+ }
+ }
+
+ if (cwBlocked && !ccwBlocked) {
+ return CodeSpinType.CCW;
+ }
+ if (!cwBlocked && ccwBlocked) {
+ return CodeSpinType.CW;
+ }
+
+ return CodeSpinType.NA;
+ }
+
+ private static double norm360(double a) {
+ double x = a % 360.0;
+ if (x < 0) x += 360.0;
+ // 360 褰掍竴鍖栦负 0
+ if (almostEqual(x, 360.0)) return 0.0;
+ return x;
+ }
+
+ private static boolean almostEqual(double a, double b) {
+ return Math.abs(a - b) <= EPS;
+ }
+
+ /**
+ * 椤烘椂閽堣窛绂伙細浠� start 鍒� end锛屾部 CW 鏂瑰悜璧板灏戝害锛岃寖鍥� (0, 360]锛岃繖閲屼笉浼氳繑鍥� 0锛堥櫎闈炰綘 curr==next 宸叉彁鍓嶅鐞嗭級
+ */
+ private static double cwDelta(double start, double end) {
+ double d = end - start;
+ if (d < 0) d += 360.0;
+ if (almostEqual(d, 0.0)) return 0.0;
+ return d;
+ }
+
+ /**
+ * 鍒ゆ柇 angle 鏄惁钀藉湪浠� start -> end 鐨勯『鏃堕拡寮ф涓婏紙涓嶅惈绔偣锛夈��
+ * 鍗筹細娌� CW 浠� start 璧� cwDelta(start,end) 鐨勮矾寰勪笂鏄惁鈥滅粡杩団�漚ngle銆�
+ */
+ private static boolean isOnArcCW(double start, double end, double angle) {
+ double total = cwDelta(start, end);
+ // 寮ч暱涓� 0 琛ㄧず鏃犳棆杞紙宸叉彁鍓嶅鐞嗭級锛岃繖閲岀洿鎺� false
+ if (almostEqual(total, 0.0)) return false;
+
+ double a = cwDelta(start, angle);
+ // 涓嶅寘鍚鐐癸細a 鍦� (0, total) 涔嬮棿鎵嶇畻缁忚繃
+ return a > EPS && a < total - EPS;
+ }
+
+ /**
+ * 鍒ゆ柇 angle 鏄惁钀藉湪浠� start -> end 鐨勯�嗘椂閽堝姬娈典笂锛堜笉鍚鐐癸級銆�
+ * 閫嗘椂閽堢瓑浠蜂簬锛氶『鏃堕拡浠� end -> start 鐨勫姬娈点��
+ */
+ private static boolean isOnArcCCW(double start, double end, double angle) {
+ // 閫嗘椂閽� start->end == 椤烘椂閽� end->start
+ return isOnArcCW(end, start, angle);
+ }
+
/**
* That vehicle is walking if the dynamic node count > 1
*/
--
Gitblit v1.9.1