From 71f29001d7ec27a72b33143dc104abd34822268a Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期二, 21 十月 2025 09:47:35 +0800
Subject: [PATCH] 更新了null值处理
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 122 +++++++++++++++++++++++-----------------
1 files changed, 69 insertions(+), 53 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..705d554 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瑙勫垝");
@@ -284,17 +269,45 @@
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());
+
+ 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 +361,7 @@
List<double[]> constraints,
Map<String, String> occupancyMap,
AGVStatus agvStatus) {
- // 棣栧厛灏濊瘯鍩烘湰璺緞瑙勫垝
+ // 灏濊瘯鍩烘湰璺緞瑙勫垝
PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
if (basicPath == null) {
return null;
@@ -359,8 +372,8 @@
basicPath, occupancyMap, agvStatus.getPhysicalConfig()
);
- // 璁剧疆璺緞鐨勬椂闂翠俊鎭�
- enhancePathWithTimeInfo(basicPath, safeStartTime, agvStatus.getPhysicalConfig());
+ // 浣跨敤缁熶竴鐨勬椂闂磋绠楀櫒璁剧疆绮剧‘鐨勬椂闂翠俊鎭�
+ timeCalculator.calculatePathTiming(basicPath, safeStartTime, agvStatus.getPhysicalConfig(), 0.0);
return basicPath;
}
@@ -392,9 +405,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 +432,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());
--
Gitblit v1.9.1