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 | 316 +++++++++++++++++++++++++++++++++++++++++++---------
1 files changed, 262 insertions(+), 54 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 9fef948..e8f0fef 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -1,26 +1,24 @@
package com.algo.service;
+import com.algo.config.EnvDataConfig;
import com.algo.model.*;
import com.algo.util.JsonUtils;
+import com.algo.util.PathTimeCalculator;
+import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;
+import javax.annotation.PostConstruct;
import java.util.*;
import java.util.concurrent.*;
/**
* 璺緞瑙勫垝鏈嶅姟
*/
+@Service
public class PathPlanningService {
- /**
- * 璺緞鏄犲皠琛�
- */
- private Map<String, Map<String, Integer>> pathMapping;
-
- /**
- * 鐜閰嶇疆
- */
- private Map<String, Object> environmentConfig;
+ @Autowired
+ private EnvDataConfig envDataConfig;
/**
* 鎵ц涓换鍔℃彁鍙�
@@ -46,64 +44,48 @@
* 鍓╀綑璺緞澶勭悊
*/
private RemainingPathProcessor remainingPathProcessor;
-
+
/**
- * 绾跨▼姹犲ぇ灏�
+ * 璺緞鏃堕棿璁＄畻鍣�
*/
- private final int threadPoolSize;
+ private PathTimeCalculator timeCalculator;
+
/**
* 绾跨▼姹�
*/
- private final ExecutorService executorService;
+ private final ExecutorService executorService = Executors.newFixedThreadPool(Math.max(4, Runtime.getRuntime().availableProcessors()));
/**
* CTU鎵瑰鐞嗗ぇ灏�
*/
private final int batchSize = 10;
- /**
- * 鏋勯�犲嚱鏁�
- *
- * @param pathMapping 璺緞鏄犲皠琛�
- * @param environmentConfig 鐜閰嶇疆
- * @param taskDataList 浠诲姟鏁版嵁鍒楄〃
- */
- public PathPlanningService(Map<String, Map<String, Integer>> pathMapping,
- Map<String, Object> environmentConfig,
- List<TaskData> taskDataList) {
- this.pathMapping = pathMapping;
- this.environmentConfig = environmentConfig;
-
- this.threadPoolSize = Math.max(4, Runtime.getRuntime().availableProcessors());
- this.executorService = Executors.newFixedThreadPool(threadPoolSize);
-
- // 鍒濆鍖�
- initializeComponents(taskDataList);
-
- System.out.println("璺緞瑙勫垝鏈嶅姟鍒濆鍖栧畬鎴愶紙绾跨▼姹犲ぇ灏�: " + threadPoolSize + "锛�");
- }
/**
* 鍒濆鍖栧悇涓粍浠�
- *
- * @param taskDataList 浠诲姟鏁版嵁鍒楄〃
*/
- private void initializeComponents(List<TaskData> taskDataList) {
- // 鍒濆鍖栦换鍔℃彁鍙栧櫒
- this.taskExtractor = new ExecutingTaskExtractor(pathMapping, taskDataList);
+ @PostConstruct
+ public void initializeComponents() {
// 鍒濆鍖栬矾寰勮鍒掑櫒
- this.pathPlanner = new AStarPathPlanner(pathMapping);
+ this.pathPlanner = new AStarPathPlanner(envDataConfig.getPathMapping());
// 鍒濆鍖栫鎾炴娴嬪櫒
- this.collisionDetector = new CollisionDetector(pathMapping);
+ this.collisionDetector = new CollisionDetector(envDataConfig.getPathMapping());
// 鍒濆鍖栫鎾炶В鍐冲櫒
this.collisionResolver = new CollisionResolver(collisionDetector);
// 鍒濆鍖栧墿浣欒矾寰勫鐞嗗櫒
- this.remainingPathProcessor = new RemainingPathProcessor(pathMapping);
+ this.remainingPathProcessor = new RemainingPathProcessor(envDataConfig.getPathMapping());
+
+ // 鍒濆鍖栨椂闂磋绠楀櫒
+ Map<String, double[]> realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
+ this.timeCalculator = new PathTimeCalculator(envDataConfig.getPathMapping(), realCoordinateMapping);
+
+ // 涓虹鎾炶В鍐冲櫒璁剧疆鏃堕棿璁＄畻鍣�
+ this.collisionResolver.setTimeCalculator(timeCalculator);
}
/**
@@ -114,9 +96,12 @@
* @param constraints 璺緞绾︽潫鏉′欢
* @return 璺緞瑙勫垝缁撴灉
*/
- public PathPlanningResult planAllAgvPaths(List<AGVStatus> agvStatusList,
+ public PathPlanningResult planAllAgvPaths(List<TaskData> taskList, List<AGVStatus> agvStatusList,
boolean includeIdleAgv,
List<double[]> constraints) {
+ // 鍒濆鍖栦换鍔℃彁鍙栧櫒
+ this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);
+
long startTime = System.currentTimeMillis();
System.out.println("寮�濮嬩负 " + agvStatusList.size() + " 涓狢TU瑙勫垝");
@@ -162,6 +147,24 @@
System.out.println("CTU " + agv.getAgvId() + " 鍓╀綑璺緞澶勭悊瀹屾垚锛岃矾寰勯暱搴�: " +
remainingPath.getCodeList().size());
}
+ }
+
+ // 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鎻愬彇鎵ц涓换鍔�
@@ -284,17 +287,51 @@
PlannedPath remainingPath = agv.getRemainingPath();
List<PathCode> remainingCodes = new ArrayList<>();
- // 浠庡綋鍓嶄綅缃紑濮嬶紝鑾峰彇鍓╀綑璺緞
+ // 鑾峰彇鍓╀綑璺緞
List<PathCode> originalCodes = remainingPath.getCodeList();
for (int i = agv.getCurrentPathIndex(); i < originalCodes.size(); i++) {
- remainingCodes.add(originalCodes.get(i));
+ PathCode originalCode = originalCodes.get(i);
+ PathCode newCode = new PathCode(originalCode.getCode(), originalCode.getDirection());
+ newCode.setActionType(originalCode.getActionType());
+ newCode.setTaskId(originalCode.getTaskId());
+ newCode.setPosType(originalCode.getPosType());
+ newCode.setLev(originalCode.getLev());
+ newCode.setTargetPoint(originalCode.isTargetPoint());
+ remainingCodes.add(newCode);
}
- // 鍒涘缓鏂扮殑璺緞瀵硅薄
PlannedPath processedPath = new PlannedPath();
processedPath.setAgvId(agv.getAgvId());
processedPath.setCodeList(remainingCodes);
- processedPath.setSegId(agv.getAgvId() + "_REMAINING_" + System.currentTimeMillis());
+
+ // 浣跨敤杈撳叆涓殑segId锛屽鏋滄病鏈夊垯鐢熸垚鏂扮殑
+ String segId = remainingPath.getSegId();
+ if (segId == null || segId.trim().isEmpty()) {
+ segId = agv.getAgvId() + "_REMAINING_" + System.currentTimeMillis();
+ }
+ processedPath.setSegId(segId);
+
+ if (timeCalculator != null && !remainingCodes.isEmpty()) {
+ // 鑾峰彇AGV鐨勪笅涓�涓矾寰勭偣鍒拌揪鏃堕棿浣滀负璧峰鏃堕棿
+ long startTime = agv.getNextPointArrivalTime();
+
+ CTUPhysicalConfig config = agv.getPhysicalConfig();
+
+ // 浼扮畻褰撳墠閫熷害锛圓GV绉诲姩涓负姝ｅ父閫熷害锛涢潤姝负0锛�
+ double initialSpeed = agv.hasRemainingPath() && agv.getRemainingPathLength() > 0
+ ? config.getNormalSpeed() : 0.0;
+
+ // 璁＄畻鏃堕棿淇℃伅
+ // arrivalTime, departureTime, cumulativeTime
+ timeCalculator.calculatePathTiming(
+ processedPath,
+ startTime,
+ config,
+ initialSpeed
+ );
+ } else {
+ System.out.println(" 鏈兘涓哄墿浣欒矾寰勮缃椂闂翠俊鎭� - AGV: " + agv.getAgvId());
+ }
return processedPath;
}
@@ -348,7 +385,7 @@
List<double[]> constraints,
Map<String, String> occupancyMap,
AGVStatus agvStatus) {
- // 棣栧厛灏濊瘯鍩烘湰璺緞瑙勫垝
+ // 灏濊瘯鍩烘湰璺緞瑙勫垝
PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
if (basicPath == null) {
return null;
@@ -359,8 +396,8 @@
basicPath, occupancyMap, agvStatus.getPhysicalConfig()
);
- // 璁剧疆璺緞鐨勬椂闂翠俊鎭�
- enhancePathWithTimeInfo(basicPath, safeStartTime, agvStatus.getPhysicalConfig());
+ // 浣跨敤缁熶竴鐨勬椂闂磋绠楀櫒璁剧疆绮剧‘鐨勬椂闂翠俊鎭�
+ timeCalculator.calculatePathTiming(basicPath, safeStartTime, agvStatus.getPhysicalConfig(), 0.0);
return basicPath;
}
@@ -392,9 +429,12 @@
// 濡傛灉鏈夋柟鍚戝彉鍖栵紝澧炲姞杞悜鏃堕棿
PathCode nextCode = codeList.get(i + 1);
- if (!pathCode.getDirection().equals(nextCode.getDirection())) {
- double turnTime = config.getTurnTime(pathCode.getDirection(), nextCode.getDirection());
- currentTime += (long) (turnTime * 1000);
+ // 娣诲姞null妫�鏌�
+ if (pathCode.getDirection() != null && nextCode.getDirection() != null) {
+ if (!pathCode.getDirection().equals(nextCode.getDirection())) {
+ double turnTime = config.getTurnTime(pathCode.getDirection(), nextCode.getDirection());
+ currentTime += (long) (turnTime * 1000);
+ }
}
}
}
@@ -416,7 +456,7 @@
long currentTime = System.currentTimeMillis() / 1000; // 杞崲涓虹
for (PathCode pathCode : codeList) {
- int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), pathMapping);
+ int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), envDataConfig.getPathMapping());
if (coord != null) {
String spaceTimeKey = coord[0] + "," + coord[1] + "," + currentTime;
occupancyMap.put(spaceTimeKey, agvStatus.getAgvId());
@@ -625,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