From 44153e246cc041180f8544e05d81f5432b14a808 Mon Sep 17 00:00:00 2001
From: vincentlu <t1341870251@gmail.com>
Date: 星期三, 04 三月 2026 07:52:45 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java | 221 +++++++++++++++++++++++++++++++++++++++++++++++++++----
1 files changed, 204 insertions(+), 17 deletions(-)
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java
index 59cbb82..1b24413 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java
@@ -3,9 +3,12 @@
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import com.zy.acs.common.utils.Utils;
import com.zy.acs.framework.common.Cools;
+import com.zy.acs.framework.exception.CoolException;
import com.zy.acs.manager.common.config.RedisProperties;
import com.zy.acs.manager.common.utils.MapDataUtils;
import com.zy.acs.manager.core.constant.MapDataConstant;
+import com.zy.acs.manager.core.domain.VehicleFootprint;
+import com.zy.acs.manager.core.service.astart.CodeNodeType;
import com.zy.acs.manager.core.service.astart.DynamicNodeType;
import com.zy.acs.manager.core.service.astart.MapDataDispatcher;
import com.zy.acs.manager.core.service.astart.NavigateNode;
@@ -33,7 +36,7 @@
import java.io.InputStreamReader;
import java.nio.file.Files;
import java.nio.file.StandardCopyOption;
-import java.util.List;
+import java.util.*;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.TimeUnit;
@@ -173,40 +176,224 @@
}
private boolean calcWaveScopeByJava(Integer lev, AgvModel agvModel) throws Exception {
- if (null == agvModel.getDiameter() || agvModel.getDiameter() <= 0) {
- log.warn("There is no diameter or diameter value was wrong...");
+ String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
+ Double[][][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
+ String[][] waveMatrix = mapDataDispatcher.initWaveMatrix(lev);
+ DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
+
+ // 璁板綍姣忓彴杞︾殑瀹夊叏鍗婂緞锛岄『甯︽爣璁板凡鎸夌煩褰㈡柟寮忕粯鍒剁殑鍦扮爜
+ Map<String, Double> vehicleAvoidDistance = new HashMap<>();
+ Set<String> preciseNodes = new HashSet<>();
+
+ // 鍏堟牴鎹疄鏃朵綅缃�+瑙掑害缁樺埗瀹為檯鐭╁舰鍗犱綅
+ List<Agv> agvList = agvService.list(new LambdaQueryWrapper<>());
+ for (Agv agv : agvList) {
+ // 姣忔鎸夎皟搴﹀垪琛ㄥ埛鏂拌溅杈嗗綋鍓嶅湴鐮佷笌瑙掑害
+ AgvDetail agvDetail = agvDetailService.selectByAgvId(agv.getId());
+ if (null == agvDetail) { continue; }
+ Long recentCodeId = agvDetail.getRecentCode();
+ if (null == recentCodeId) { continue; }
+ Code centerCode = codeService.getCacheById(recentCodeId);
+ if (null == centerCode) { continue; }
+
+ AgvModel model = agvModelService.getByAgvId(agv.getId());
+ if (null == model) {
+ model = agvModel;
+ }
+ double avoidDistance = resolveAvoidDistance(model, agvModel);
+ vehicleAvoidDistance.put(agv.getUuid(), avoidDistance);
+
+ // 灏嗘棆杞煩褰㈡槧灏勫埌缃戞牸锛屽緱鍒伴渶瑕佸崰鐢ㄧ殑鑺傜偣闆嗗悎
+ List<NavigateNode> footprintNodes = calcFootprintNodes(lev, centerCode, agvDetail, model, codeMatrix, cdaMatrix);
+ if (Cools.isEmpty(footprintNodes)) {
+ continue;
+ }
+ preciseNodes.add(centerCode.getData());
+ for (NavigateNode navigateNode : footprintNodes) {
+ String waveNode = waveMatrix[navigateNode.getX()][navigateNode.getY()];
+ waveMatrix[navigateNode.getX()][navigateNode.getY()] = MapDataUtils.generateWaveNode(waveNode, agv.getUuid());
+ }
}
- Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getDiameter(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
-
- // java
- String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
- String[][] waveMatrix = mapDataDispatcher.initWaveMatrix(lev);
-
- // lock path
- DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
+ // 鍐嶇敤鍘熸潵鐨勫渾褰㈡尝绾硅ˉ涓婂墿浣欑殑璺緞閿佸畾鑺傜偣
for (int i = 0; i < dynamicMatrix.length; i++) {
for (int j = 0; j < dynamicMatrix[i].length; j++) {
DynamicNode dynamicNode = dynamicMatrix[i][j];
String vehicle = dynamicNode.getVehicle();
if (!DynamicNodeType.ACCESS.val.equals(vehicle) && !DynamicNodeType.BLOCK.val.equals(vehicle)) {
-
- List<NavigateNode> includeList = mapService.getWaveScopeByCode(lev, codeMatrix[i][j], avoidDistance);
-
+ String codeData = codeMatrix[i][j];
+ if (preciseNodes.contains(codeData)) {
+ // 宸茬粡鎸夌湡瀹炵煩褰㈠啓杩囦竴娆★紝閬垮厤閲嶅鍗犵敤
+ continue;
+ }
+ double avoidDistance = vehicleAvoidDistance.getOrDefault(vehicle, resolveAvoidDistance(null, agvModel));
+ if (avoidDistance <= 0) {
+ continue;
+ }
+ List<NavigateNode> includeList = mapService.getWaveScopeByCode(lev, codeData, avoidDistance);
for (NavigateNode navigateNode : includeList) {
- String waveNode = waveMatrix[navigateNode.getX()][navigateNode.getY()]; // overlay
+ String waveNode = waveMatrix[navigateNode.getX()][navigateNode.getY()];
waveMatrix[navigateNode.getX()][navigateNode.getY()] = MapDataUtils.generateWaveNode(waveNode, vehicle);
}
}
}
}
-// mapDataDispatcher.printMatrix(waveMatrix);
+ // 鎵�鏈夎溅杈嗗悓姝ュ畬鎴愬悗鍐嶇粺涓�鍐欏洖 redis锛屽噺灏戦绻佸埛鏂�
mapDataDispatcher.setWaveMatrix(lev, waveMatrix);
-
return true;
}
+ // 涓嶅悓杞﹀瀷鍙兘鐩村緞涓嶅悓锛岀己澶辨椂閫�鍥炶皟鐢ㄦ柟浼犲叆鐨勯粯璁ゆā鍨嬶紝鏈�鍚庡啀閫�鍥� 0
+ private double resolveAvoidDistance(AgvModel agvModel, AgvModel fallback) {
+ Integer diameter = null;
+ if (null != agvModel && null != agvModel.getDiameter() && agvModel.getDiameter() > 0) {
+ diameter = agvModel.getDiameter();
+ } else if (null != fallback) {
+ diameter = fallback.getDiameter();
+ }
+ return MapDataUtils.getVehicleWaveSafeDistance(diameter, MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
+ }
+
+ /**
+ * 璁＄畻缁欏畾杞﹁締鍦ㄥ綋鍓嶈搴︿笅鐪熷疄瑕嗙洊鐨勭綉鏍艰妭鐐�
+ */
+ private List<NavigateNode> calcFootprintNodes(Integer lev, Code centerCode, AgvDetail agvDetail, AgvModel agvModel
+ , String[][] codeMatrix, Double[][][] cdaMatrix) {
+ if (null == centerCode || null == agvDetail || null == agvModel) {
+ return new ArrayList<>();
+ }
+
+ int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, centerCode.getData());
+ if (null == codeMatrixIdx) {
+ return new ArrayList<>();
+ }
+ Double centerX = cdaMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]][0];
+ Double centerY = cdaMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]][1];
+ if (centerX == null || centerY == null) {
+ return new ArrayList<>();
+ }
+
+ VehicleFootprint footprint = this.buildFootprint(agvModel);
+ if (null == footprint) {
+ return new ArrayList<>();
+ }
+
+ double direction = Optional.ofNullable(agvDetail.getAgvAngle()).orElse(0D);
+ // 鐜板満瑙掑害鏄互姝ｅ寳涓� 0锛� 缁熶竴杞崲鎴愬姬搴︽柟渚夸笁瑙掑嚱鏁�
+ double headingRad = Math.toRadians(90 - direction);
+
+ // 鏃嬭浆鐭╁舰鍏堟眰鍑轰笘鐣屽潗鏍囧寘鍥寸洅锛屽噺灏戦亶鍘�
+ double[][] corners = this.buildRectangleCorners(centerX, centerY, footprint, headingRad);
+ double minX = Arrays.stream(corners).mapToDouble(point -> point[0]).min().orElse(centerX);
+ double maxX = Arrays.stream(corners).mapToDouble(point -> point[0]).max().orElse(centerX);
+ double minY = Arrays.stream(corners).mapToDouble(point -> point[1]).min().orElse(centerY);
+ double maxY = Arrays.stream(corners).mapToDouble(point -> point[1]).max().orElse(centerY);
+
+ List<NavigateNode> nodeList = new ArrayList<>();
+ for (int i = 0; i < codeMatrix.length; i++) {
+ for (int j = 0; j < codeMatrix[i].length; j++) {
+ String codeData = codeMatrix[i][j];
+ if (CodeNodeType.NONE.val.equals(codeData)) {
+ continue;
+ }
+ Double px = cdaMatrix[i][j][0];
+ Double py = cdaMatrix[i][j][1];
+ if (px == null || py == null) {
+ continue;
+ }
+ // 鍏堝埄鐢� AABB 鍋氫竴娆＄矖婊わ紝鍐嶅仛绮剧‘鐨勭偣鍐呮娴�
+ if (px < minX || px > maxX || py < minY || py > maxY) {
+ continue;
+ }
+ // 鏄惁鍛戒腑 褰㈢姸鐭╅樀鑼冨洿
+ if (this.isInsideFootprint(px, py, centerX, centerY, footprint, headingRad)) {
+ nodeList.add(new NavigateNode(i, j, codeData));
+ }
+ }
+ }
+ return nodeList;
+ }
+
+ /**
+ * 鏍规嵁杞﹀瀷鍙傛暟锛堝惈 head/tail/width锛夋瀯寤烘姇褰辩敤鐨勭煩褰紝蹇呰鏃堕檷绾т娇鐢� length
+ */
+ private VehicleFootprint buildFootprint(AgvModel agvModel) {
+ if (null == agvModel) {
+ return null;
+ }
+// double baseLength = Optional.ofNullable(agvModel.getLength()).filter(len -> len > 0).map(Double::valueOf).orElse(0D);
+// double defaultHalfLength = baseLength > 0 ? baseLength / 2.0 : 0;
+
+ if (null == agvModel.getHeadOffset() || agvModel.getHeadOffset() <= 0) {
+ throw new CoolException(agvModel.getName() + "鏈缃� 杞﹀ご闀垮害銆�");
+ }
+ if (null == agvModel.getTailOffset() || agvModel.getTailOffset() <= 0) {
+ throw new CoolException(agvModel.getName() + "鏈缃� 杞﹀熬闀垮害銆�");
+ }
+ if (null == agvModel.getWidth() || agvModel.getWidth() <= 0) {
+ throw new CoolException(agvModel.getName() + "鏈缃� 杞﹁韩瀹藉害銆�");
+ }
+ double head = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getHeadOffset(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
+ double tail = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getTailOffset(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
+ double halfWidth = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getWidth() / 2, MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
+
+ // padding 鐩存帴娌垮洓涓柟鍚戠瓑姣斾緥澶栨墿锛岀瓑浠蜂簬缁欒溅韬鐣欏畨鍏ㄨ窛绂�
+// double padding = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getDiameter(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR) / 2.0;
+// if (padding > 0) {
+// head += padding;
+// tail += padding;
+// halfWidth += padding;
+// }
+
+// head = head <= 0 ? Math.max(tail, halfWidth) : head;
+// tail = tail <= 0 ? head : tail;
+// halfWidth = halfWidth <= 0 ? Math.max(head, tail) / 2.0 : halfWidth;
+//
+// if (head <= 0 || tail <= 0 || halfWidth <= 0) {
+// return null;
+// }
+ return new VehicleFootprint(head, tail, halfWidth);
+ }
+
+ /**
+ * 鏃嬭浆鐭╁舰鐨勫洓涓鍧愭爣锛岀敤鏉ュ揩閫熺敓鎴愬寘鍥寸洅
+ */
+ private double[][] buildRectangleCorners(double centerX, double centerY, VehicleFootprint footprint, double headingRad) {
+ double cos = Math.cos(headingRad);
+ double sin = Math.sin(headingRad);
+
+ double[][] localPoints = new double[][]{
+ {footprint.head, footprint.halfWidth},
+ {footprint.head, -footprint.halfWidth},
+ {-footprint.tail, footprint.halfWidth},
+ {-footprint.tail, -footprint.halfWidth}
+ };
+
+ double[][] corners = new double[4][2];
+ for (int idx = 0; idx < localPoints.length; idx++) {
+ double lx = localPoints[idx][0];
+ double ly = localPoints[idx][1];
+ double gx = centerX + lx * cos - ly * sin;
+ double gy = centerY + lx * sin + ly * cos;
+ corners[idx][0] = gx;
+ corners[idx][1] = gy;
+ }
+ return corners;
+ }
+
+ private boolean isInsideFootprint(double px, double py, double centerX, double centerY, VehicleFootprint footprint, double headingRad) {
+ double dx = px - centerX;
+ double dy = py - centerY;
+
+ // 鎶婄偣缁曞洖杞﹁締鏈綋鐨勫眬閮ㄥ潗鏍囷紝鍐嶅仛绠�鍗曠煩褰㈠垽鏂�
+ double localX = dx * Math.cos(headingRad) + dy * Math.sin(headingRad);
+ double localY = -dx * Math.sin(headingRad) + dy * Math.cos(headingRad);
+
+ return localX >= -footprint.tail && localX <= footprint.head
+ && localY >= -footprint.halfWidth && localY <= footprint.halfWidth;
+ }
+
private File loadPythonFile() {
File scriptFile = null;
try {
--
Gitblit v1.9.1