优化更新

jianghaiyue

2025-11-04 5bcf58fa72ec1a03791fb238399986b44bc0ec26

优化更新

 algo-zkd/src/main/java/com/algo/service/PathPlanningService.java

@@ -864,8 +864,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<>(); // 记录位置和距离
@@ -889,19 +896,108 @@
                    continue;
                }
                
                // 找到一个未被占用的位置
                if (!blockedPositions.contains(neighborPos) && (depth + 1) >= 3) {
                    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;
    }