algo-java.git - Gitblit

			@@ -149,6 +149,24 @@
			}
			}

			// 3.5. 检查空闲AGV是否需要让行
			System.out.println("检查空闲AGV是否需要让行");
			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(" 无需让行的AGV");
			}

			// 4. 为需要新路径的CTU提取执行中任务
			System.out.println("步骤3: 提取需要新路径的CTU任务");
			List<ExecutingTask> newTasks = taskExtractor.extractExecutingTasks(ctuNeedingNewPaths);
			@@ -269,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();

			// 估算当前速度（AGV移动中为正常速度；静止为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;
			}
			@@ -377,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);
			}
			}
			}
			}
			@@ -610,6 +665,174 @@
			}

			/**
			* 识别需要让行的AGV
			* 检查空闲AGV是否占用了其他AGV剩余路径上的位置
			*
			* @param agvStatusList 所有AGV状态列表
			* @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());
			}
			}
			}
			}

			// 检查每个AGV
			for (AGVStatus agv : agvStatusList) {
			// 如果这个AGV已经有剩余路径，跳过（已在任务）
			if (agv.hasRemainingPath()) {
			continue;
			}

			// 检查AGV是否有位置信息
			if (agv.getPosition() == null \|\| agv.getPosition().trim().isEmpty()) {
			continue;
			}

			// 检查AGV是否是空闲状态（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 + " 占用了其他CTU路径，需要让行");
			yieldingAgvs.add(agv);
			}
			}

			return yieldingAgvs;
			}

			/**
			* 为让行AGV规划避让路径
			*
			* @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 {