algo-java.git - Gitblit

			@@ -103,8 +103,16 @@
			this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);

			long startTime = System.currentTimeMillis();
			long unifiedTimestamp = startTime / 1000; // 统一时间戳（秒）

			System.out.println("开始为 " + agvStatusList.size() + " 个CTU规划");
			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: 构建时空占用表");
			@@ -228,12 +236,40 @@
			// 6. 处理空闲CTU
			if (includeIdleAgv) {
			System.out.println("步骤5: 处理空闲CTU");

			// 6.1. 检查空闲AGV是否占用已规划路径，如果占用则生成避让路径
			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(" 无需避让的空闲AGV");
			}

			// 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: 最终碰撞检测");
			@@ -385,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);

			// 使用统一的时间计算器设置精确的时间信息
			timeCalculator.calculatePathTiming(basicPath, safeStartTime, agvStatus.getPhysicalConfig(), 0.0);

			return basicPath;
			return spacetimePath;
			}

			/**
			@@ -665,6 +703,66 @@
			}

			/**
			* 迭代检查所有空闲AGV是否占用已规划路径
			*/
			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(" 无需避让的AGV");
			} 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 + " 个避让AGV，迭代 " + iteration + " 轮");
			}
			}

			/**
			* 识别需要让行的AGV
			* 检查空闲AGV是否占用了其他AGV剩余路径上的位置
			*
			@@ -776,8 +874,15 @@
			* 寻找让行的目标位置
			*/
			private String findYieldTargetPosition(String currentPos, Set<String> blockedPositions, AGVStatus agv) {
			// 使用BFS搜索最近的空闲位置
			final int MAX_SEARCH_DEPTH = 10;
			// 使用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<>(); // 记录位置和距离
			@@ -801,19 +906,108 @@
			continue;
			}

			// 找到一个未被占用的位置
			if (!blockedPositions.contains(neighborPos)) {
			System.out.println(" 找到避让位置: " + neighborPos + " (距离=" + (depth + 1) + "步)");
			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);
			visited.put(neighborPos, depth + 1);
			}
			}

			System.out.println(" 未找到合适的避让位置 ");
			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;
			}