From 434e0ff84a7df875f223e9118f244e04f28310ca Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期二, 04 十一月 2025 14:38:56 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 468 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
1 files changed, 449 insertions(+), 19 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 833510e..61d625d 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -3,6 +3,7 @@
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;
@@ -43,6 +44,11 @@
* 鍓╀綑璺緞澶勭悊
*/
private RemainingPathProcessor remainingPathProcessor;
+
+ /**
+ * 璺緞鏃堕棿璁＄畻鍣�
+ */
+ private PathTimeCalculator timeCalculator;
/**
@@ -73,6 +79,13 @@
// 鍒濆鍖栧墿浣欒矾寰勫鐞嗗櫒
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);
}
/**
@@ -90,8 +103,16 @@
this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);
long startTime = System.currentTimeMillis();
+ long unifiedTimestamp = startTime / 1000; // 缁熶竴鏃堕棿鎴筹紙绉掞級
System.out.println("寮�濮嬩负 " + agvStatusList.size() + " 涓狢TU瑙勫垝");
+ System.out.println("缁熶竴鏃堕棿鍩哄噯: " + unifiedTimestamp + "绉� (" + new Date(startTime) + ")");
+
+ // 缁熻鏁呴殰杞︽暟閲�
+ long faultyCount = agvStatusList.stream().filter(agv -> agv.getError() == 1).count();
+ if (faultyCount > 0) {
+ System.out.println("妫�娴嬪埌 " + faultyCount + " 涓晠闅滆溅(error=1)锛屽皢鍦ㄦ椂绌鸿〃涓崰鎹綅缃�");
+ }
// 1. 鏋勫缓鐜版湁鍓╀綑璺緞鐨勬椂绌哄崰鐢ㄨ〃
System.out.println("姝ラ1: 鏋勫缓鏃剁┖鍗犵敤琛�");
@@ -134,6 +155,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鎻愬彇鎵ц涓换鍔�
@@ -197,12 +236,40 @@
// 6. 澶勭悊绌洪棽CTU
if (includeIdleAgv) {
System.out.println("姝ラ5: 澶勭悊绌洪棽CTU");
+
+ // 6.1. 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤宸茶鍒掕矾寰勶紝濡傛灉鍗犵敤鍒欑敓鎴愰伩璁╄矾寰�
+ List<AGVStatus> yieldingIdleAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
+
+ List<AGVStatus> yieldingAgvsToHandle = new ArrayList<>();
+ for (AGVStatus yieldAgv : yieldingIdleAgvs) {
+ if (!plannedAgvIds.containsKey(yieldAgv.getAgvId())) {
+ yieldingAgvsToHandle.add(yieldAgv);
+ }
+ }
+
+ if (!yieldingAgvsToHandle.isEmpty()) {
+ System.out.println( yieldingAgvsToHandle.size() + " 涓渶瑕侀伩璁╃殑绌洪棽AGV");
+ for (AGVStatus yieldAgv : yieldingAgvsToHandle) {
+ PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
+ if (yieldPath != null) {
+ plannedPaths.add(yieldPath);
+ plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
+ }
+ }
+ } else {
+ System.out.println(" 鏃犻渶閬胯鐨勭┖闂睞GV");
+ }
+
+ // 6.2. 鐒跺悗涓哄墿浣欑殑绌洪棽AGV瑙勫垝寰呮満/鍏呯數璺緞
List<PlannedPath> idlePaths = planIdleAgvPathsWithConstraints(
ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap
);
plannedPaths.addAll(idlePaths);
- System.out.println("绌洪棽CTU璺緞瑙勫垝瀹屾垚锛屾暟閲�: " + idlePaths.size());
+ System.out.println(" 绌洪棽CTU璺緞瑙勫垝锛屾暟閲�: " + idlePaths.size());
}
+
+ // 6.5. 鏈�缁堥伩璁╂鏌�
+ performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints);
// 7. 鏈�缁堢鎾炴娴嬪拰瑙ｅ喅锛堥拡瀵瑰悓鏃剁敓鎴愮殑鏂拌矾寰勶級
System.out.println("姝ラ6: 鏈�缁堢鎾炴娴�");
@@ -256,17 +323,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;
}
@@ -320,21 +421,23 @@
List<double[]> constraints,
Map<String, String> occupancyMap,
AGVStatus agvStatus) {
- // 棣栧厛灏濊瘯鍩烘湰璺緞瑙勫垝
- PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
- if (basicPath == null) {
+ // 甯︽椂绌哄崰鐢ㄨ〃鐨勮矾寰勮鍒�
+ PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
+ startPos,
+ endPos,
+ constraints,
+ occupancyMap,
+ agvStatus.getPhysicalConfig()
+ );
+
+ if (spacetimePath == null) {
return null;
}
- // 鎵惧埌瀹夊叏鐨勮捣濮嬫椂闂�
- long safeStartTime = remainingPathProcessor.findSafeStartTime(
- basicPath, occupancyMap, agvStatus.getPhysicalConfig()
- );
+ long startTime = System.currentTimeMillis();
+ timeCalculator.calculatePathTiming(spacetimePath, startTime, agvStatus.getPhysicalConfig(), 0.0);
- // 璁剧疆璺緞鐨勬椂闂翠俊鎭�
- enhancePathWithTimeInfo(basicPath, safeStartTime, agvStatus.getPhysicalConfig());
-
- return basicPath;
+ return spacetimePath;
}
/**
@@ -364,9 +467,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);
+ }
}
}
}
@@ -597,6 +703,330 @@
}
/**
+ * 杩唬妫�鏌ユ墍鏈夌┖闂睞GV鏄惁鍗犵敤宸茶鍒掕矾寰�
+ */
+ private void performFinalYieldingCheck(List<AGVStatus> agvStatusList,
+ List<PlannedPath> plannedPaths,
+ Map<String, String> plannedAgvIds,
+ Map<String, String> spaceTimeOccupancyMap,
+ List<double[]> constraints) {
+ final int MAX_ITERATIONS = 5; // 鏈�澶ц凯浠ｆ鏁�
+ int iteration = 0;
+ int totalYieldingCount = 0;
+
+ while (iteration < MAX_ITERATIONS) {
+ iteration++;
+
+ // 璇嗗埆闇�瑕侀伩璁╃殑绌洪棽AGV
+ List<AGVStatus> yieldingAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
+
+ if (yieldingAgvs.isEmpty()) {
+ if (iteration == 1) {
+ System.out.println(" 鏃犻渶閬胯鐨凙GV");
+ } else {
+ System.out.println(" 绗�" + iteration + "杞鏌ワ細鏃犳柊鐨勯伩璁╅渶姹�");
+ }
+ break;
+ }
+
+ System.out.println(" 绗�" + iteration + "杞鏌ワ細" + yieldingAgvs.size() + " 涓渶瑕侀伩璁╃殑AGV");
+
+ // 瑙勫垝閬胯璺緞
+ int successCount = 0;
+ for (AGVStatus yieldAgv : yieldingAgvs) {
+ if (plannedAgvIds.containsKey(yieldAgv.getAgvId())) {
+ continue;
+ }
+
+ PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
+ if (yieldPath != null) {
+ plannedPaths.add(yieldPath);
+ plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
+ successCount++;
+ totalYieldingCount++;
+ }
+ }
+
+ if (successCount == 0) {
+ System.out.println(" 绗�" + iteration + "杞鏌ワ細鏃犳硶涓洪渶瑕侀伩璁╃殑AGV瑙勫垝璺緞锛屽仠姝㈣凯浠�");
+ break;
+ }
+ }
+
+ if (iteration >= MAX_ITERATIONS) {
+ System.out.println(" 杈惧埌鏈�澶ц凯浠ｆ鏁�(" + MAX_ITERATIONS + ")锛屽彲鑳藉瓨鍦ㄥ鏉傜殑閬胯鍦烘櫙");
+ }
+
+ if (totalYieldingCount > 0) {
+ System.out.println(" 閬胯妫�鏌ュ畬鎴愶細鍏卞鐞� " + totalYieldingCount + " 涓伩璁〢GV锛岃凯浠� " + iteration + " 杞�");
+ }
+ }
+
+ /**
+ * 璇嗗埆闇�瑕佽琛岀殑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 = 15;
+
+ // 鑾峰彇瀹夊叏璺濈鍙傛暟锛堜粠鐗╃悊閰嶇疆涓幏鍙栵紝榛樿3.0锛�
+ CTUPhysicalConfig config = agv.getPhysicalConfig();
+ double minSafetyDistance = config != null ? config.getMinSafetyDistance() : 3.0;
+
+ // 灏嗗畨鍏ㄨ窛绂昏浆鎹负缃戞牸姝ユ暟锛堝亣璁炬瘡涓妭鐐归棿璺濅负1锛�
+ int safetySteps = (int) Math.ceil(minSafetyDistance);
+
+ 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;
+ }
+
+ int distanceToPos = depth + 1;
+ visited.put(neighborPos, distanceToPos);
+
+ // 妫�鏌ヨ浣嶇疆鏄惁婊¤冻瑕佹眰
+ if (!blockedPositions.contains(neighborPos) &&
+ isSafeDistanceFromBlockedPositions(neighborPos, blockedPositions, safetySteps)) {
+ System.out.println(" 鎵惧埌瀹夊叏閬胯浣嶇疆: " + neighborPos);
+ return neighborPos;
+ }
+
+ // 鍔犲叆闃熷垪缁х画鎼滅储
+ queue.offer(neighborPos);
+ }
+ }
+
+ System.out.println(" 鏈壘鍒版弧瓒冲畨鍏ㄨ窛绂荤殑閬胯浣嶇疆锛岄檷浣庡畨鍏ㄨ姹傞噸璇�");
+
+ // 濡傛灉鎵句笉鍒版弧瓒冲畨鍏ㄨ窛绂荤殑浣嶇疆锛岄檷浣庤姹傚啀娆℃悳绱紙鑷冲皯淇濊瘉2姝ヨ窛绂伙級
+ return findYieldTargetPositionWithReducedSafety(currentPos, blockedPositions, 2);
+ }
+
+ /**
+ * 妫�鏌ュ�欓�変綅缃笌鎵�鏈夎鍗犵敤浣嶇疆鏄惁婊¤冻瀹夊叏璺濈
+ */
+ private boolean isSafeDistanceFromBlockedPositions(String candidatePos,
+ Set<String> blockedPositions,
+ int minSteps) {
+ // 鑾峰彇鍊欓�変綅缃殑鍧愭爣
+ int[] candidateCoord = JsonUtils.getCoordinate(candidatePos, envDataConfig.getPathMapping());
+ if (candidateCoord == null) {
+ return false;
+ }
+
+ // 妫�鏌ヤ笌姣忎釜琚崰鐢ㄤ綅缃殑璺濈
+ for (String blockedPos : blockedPositions) {
+ int[] blockedCoord = JsonUtils.getCoordinate(blockedPos, envDataConfig.getPathMapping());
+ if (blockedCoord == null) {
+ continue;
+ }
+
+ // 璁＄畻鏇煎搱椤胯窛绂�
+ int distance = Math.abs(candidateCoord[0] - blockedCoord[0]) +
+ Math.abs(candidateCoord[1] - blockedCoord[1]);
+
+ if (distance < minSteps) {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ /**
+ * 浣跨敤闄嶄綆鐨勫畨鍏ㄨ姹傛煡鎵捐琛屼綅缃紙澶囬�夛級
+ *
+ * @param currentPos 褰撳墠浣嶇疆
+ * @param blockedPositions 琚崰鐢ㄧ殑浣嶇疆闆嗗悎
+ * @param minSteps 鏈�灏忔鏁拌姹�
+ * @return 璁╄鐩爣浣嶇疆
+ */
+ private String findYieldTargetPositionWithReducedSafety(String currentPos,
+ Set<String> blockedPositions,
+ int minSteps) {
+ 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;
+ }
+
+ visited.put(neighborPos, depth + 1);
+
+ // 浣跨敤闄嶄綆鐨勫畨鍏ㄨ姹�
+ if (!blockedPositions.contains(neighborPos) &&
+ isSafeDistanceFromBlockedPositions(neighborPos, blockedPositions, minSteps)) {
+ System.out.println(" 鎵惧埌闄嶇骇瀹夊叏閬胯浣嶇疆: " + neighborPos +
+ " (璺濈=" + (depth + 1) + "姝�, 鏈�灏忓畨鍏ㄨ窛绂�=" + minSteps + "姝�)");
+ return neighborPos;
+ }
+
+ queue.offer(neighborPos);
+ }
+ }
+
+ 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