From 1d520bffcd63cb8389c9cdf719c3cf7e7c4af567 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期四, 30 十月 2025 10:20:28 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 186 ++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 186 insertions(+), 0 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
index b1e681a..e8f0fef 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -149,6 +149,24 @@
}
}
+ // 3.5. 妫�鏌ョ┖闂睞GV鏄惁闇�瑕佽琛�
+ System.out.println("妫�鏌ョ┖闂睞GV鏄惁闇�瑕佽琛�");
+ List<AGVStatus> yieldingAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
+ if (!yieldingAgvs.isEmpty()) {
+ System.out.println(" 鍙戠幇 " + yieldingAgvs.size() + " 涓渶瑕佽琛岀殑绌洪棽AGV");
+ for (AGVStatus yieldAgv : yieldingAgvs) {
+ PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
+ if (yieldPath != null) {
+ plannedPaths.add(yieldPath);
+ plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
+ System.out.println(" AGV " + yieldAgv.getAgvId() + " 璁╄璺緞瑙勫垝鎴愬姛锛屼粠 " +
+ yieldAgv.getPosition() + " 绉诲紑");
+ }
+ }
+ } else {
+ System.out.println(" 鏃犻渶璁╄鐨凙GV");
+ }
+
// 4. 涓洪渶瑕佹柊璺緞鐨凜TU鎻愬彇鎵ц涓换鍔�
System.out.println("姝ラ3: 鎻愬彇闇�瑕佹柊璺緞鐨凜TU浠诲姟");
List<ExecutingTask> newTasks = taskExtractor.extractExecutingTasks(ctuNeedingNewPaths);
@@ -647,6 +665,174 @@
}
/**
+ * 璇嗗埆闇�瑕佽琛岀殑AGV
+ * 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤浜嗗叾浠朅GV鍓╀綑璺緞涓婄殑浣嶇疆
+ *
+ * @param agvStatusList 鎵�鏈堿GV鐘舵�佸垪琛�
+ * @param plannedPaths 宸茶鍒掔殑璺緞鍒楄〃锛堝寘鍚墿浣欒矾寰勶級
+ * @return 闇�瑕佽琛岀殑AGV鍒楄〃
+ */
+ private List<AGVStatus> identifyYieldingAgvs(List<AGVStatus> agvStatusList, List<PlannedPath> plannedPaths) {
+ List<AGVStatus> yieldingAgvs = new ArrayList<>();
+
+ // 鏀堕泦鎵�鏈夊凡瑙勫垝璺緞涓婄殑浣嶇疆
+ Set<String> occupiedPositions = new HashSet<>();
+ for (PlannedPath path : plannedPaths) {
+ if (path.getCodeList() != null) {
+ for (PathCode code : path.getCodeList()) {
+ if (code.getCode() != null) {
+ occupiedPositions.add(code.getCode());
+ }
+ }
+ }
+ }
+
+ // 妫�鏌ユ瘡涓狝GV
+ for (AGVStatus agv : agvStatusList) {
+ // 濡傛灉杩欎釜AGV宸茬粡鏈夊墿浣欒矾寰勶紝璺宠繃锛堝凡鍦ㄤ换鍔★級
+ if (agv.hasRemainingPath()) {
+ continue;
+ }
+
+ // 妫�鏌GV鏄惁鏈変綅缃俊鎭�
+ if (agv.getPosition() == null || agv.getPosition().trim().isEmpty()) {
+ continue;
+ }
+
+ // 妫�鏌GV鏄惁鏄┖闂茬姸鎬侊紙status=0锛�
+ // 鍙娌℃湁鍓╀綑璺緞涓旀湁浣嶇疆锛屽氨妫�鏌ユ槸鍚﹂渶瑕佽琛�
+ int status = agv.getStatus();
+ if (status != 0) {
+ // status=1鎴�2锛堥棶棰樻垨蹇欑锛夌瓑鍏朵粬鐘舵�佽烦杩�
+ continue;
+ }
+
+ // 妫�鏌ヨAGV鐨勪綅缃槸鍚﹀湪鍏朵粬AGV鐨勮矾寰勪笂
+ String currentPos = agv.getPosition();
+ if (occupiedPositions.contains(currentPos)) {
+ System.out.println(" CTU " + agv.getAgvId() +
+ " (status=" + status + ") 鍦ㄤ綅缃� " + currentPos + " 鍗犵敤浜嗗叾浠朇TU璺緞锛岄渶瑕佽琛�");
+ yieldingAgvs.add(agv);
+ }
+ }
+
+ return yieldingAgvs;
+ }
+
+ /**
+ * 涓鸿琛孉GV瑙勫垝閬胯璺緞
+ *
+ * @param yieldAgv 闇�瑕佽琛岀殑AGV
+ * @param existingPaths 宸插瓨鍦ㄧ殑璺緞鍒楄〃
+ * @param occupancyMap 鏃剁┖鍗犵敤琛�
+ * @param constraints 璺緞绾︽潫
+ * @return 璁╄璺緞
+ */
+ private PlannedPath planYieldPath(AGVStatus yieldAgv, List<PlannedPath> existingPaths,
+ Map<String, String> occupancyMap, List<double[]> constraints) {
+ String currentPos = yieldAgv.getPosition();
+ String agvId = yieldAgv.getAgvId();
+
+ // 1. 鎵惧埌瀹夊叏鐨勭洰鏍囦綅缃�
+ Set<String> blockedPositions = new HashSet<>();
+ for (PlannedPath path : existingPaths) {
+ if (path.getCodeList() != null) {
+ for (PathCode code : path.getCodeList()) {
+ if (code.getCode() != null) {
+ blockedPositions.add(code.getCode());
+ }
+ }
+ }
+ }
+
+ // 2. 瀵绘壘鍚堥�傝琛岀洰鏍囦綅缃�
+ String targetPos = findYieldTargetPosition(currentPos, blockedPositions, yieldAgv);
+
+ if (targetPos == null || targetPos.equals(currentPos)) {
+ System.out.println(" AGV " + agvId + " 鏃犳硶鎵惧埌鍚堥�傜殑璁╄浣嶇疆");
+ return null;
+ }
+
+ // 3. 瑙勫垝閬胯璺緞
+ PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
+ currentPos, targetPos, constraints, occupancyMap, yieldAgv
+ );
+
+ if (yieldPath != null) {
+ yieldPath.setAgvId(agvId);
+ yieldPath.setSegId(generateSegId("AVOID", agvId, "avoiding"));
+
+ // 璁剧疆璺緞浠ｇ爜淇℃伅
+ enhancePathCodesForYielding(yieldPath);
+
+ // 鏇存柊鍗犵敤琛�
+ updateSpaceTimeOccupancyMap(yieldPath, occupancyMap, yieldAgv);
+ }
+
+ return yieldPath;
+ }
+
+ /**
+ * 瀵绘壘璁╄鐨勭洰鏍囦綅缃�
+ */
+ private String findYieldTargetPosition(String currentPos, Set<String> blockedPositions, AGVStatus agv) {
+ // 浣跨敤BFS鎼滅储鏈�杩戠殑绌洪棽浣嶇疆
+ final int MAX_SEARCH_DEPTH = 10;
+
+ Queue<String> queue = new LinkedList<>();
+ Map<String, Integer> visited = new HashMap<>(); // 璁板綍浣嶇疆鍜岃窛绂�
+
+ queue.offer(currentPos);
+ visited.put(currentPos, 0);
+
+ while (!queue.isEmpty()) {
+ String pos = queue.poll();
+ int depth = visited.get(pos);
+
+ if (depth >= MAX_SEARCH_DEPTH) {
+ break;
+ }
+
+ List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
+ for (Map<String, String> neighbor : neighbors) {
+ String neighborPos = neighbor.get("code");
+
+ if (neighborPos == null || visited.containsKey(neighborPos)) {
+ continue;
+ }
+
+ // 鎵惧埌涓�涓湭琚崰鐢ㄧ殑浣嶇疆
+ if (!blockedPositions.contains(neighborPos)) {
+ System.out.println(" 鎵惧埌閬胯浣嶇疆: " + neighborPos + " (璺濈=" + (depth + 1) + "姝�)");
+ return neighborPos;
+ }
+
+ // 鍔犲叆闃熷垪缁х画鎼滅储
+ queue.offer(neighborPos);
+ visited.put(neighborPos, depth + 1);
+ }
+ }
+
+ System.out.println(" 鏈壘鍒板悎閫傜殑閬胯浣嶇疆 ");
+ return null;
+ }
+
+ private void enhancePathCodesForYielding(PlannedPath yieldPath) {
+ if (yieldPath == null || yieldPath.getCodeList() == null) {
+ return;
+ }
+
+ List<PathCode> codes = yieldPath.getCodeList();
+ for (int i = 0; i < codes.size(); i++) {
+ PathCode code = codes.get(i);
+ code.setActionType("0");
+ code.setPosType(null);
+ code.setLev(0);
+ code.setTargetPoint(i == codes.size() - 1);
+ }
+ }
+
+ /**
* 璺緞瑙勫垝缁撴灉绫�
*/
public static class PathPlanningResult {
--
Gitblit v1.9.1