优化更新

jianghaiyue

2025-11-04 434e0ff84a7df875f223e9118f244e04f28310ca

优化更新

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

@@ -44,7 +44,7 @@
    /**
     * 最大搜索深度
     */
    private final int maxSearchDepth = 15000;
    private final int maxSearchDepth = 50000;

    /**
     * 距离缓存

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

@@ -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: 构建时空占用表");
@@ -413,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;
    }

    /**

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

@@ -26,33 +26,79 @@
     * 系统当前时间基准（毫秒）
     */
    private long systemBaseTime;
    private long unifiedTimestamp;

    public RemainingPathProcessor(Map<String, Map<String, Integer>> pathMapping) {
        this.pathMapping = pathMapping;
        this.systemBaseTime = System.currentTimeMillis();
        this.unifiedTimestamp = this.systemBaseTime / 1000; 
    }

    /**
     * 处理所有CTU的剩余路径，构建时空占用表
     * 🔧 修复：同时处理有剩余路径的AGV和静止AGV的当前位置占用
     * 1. 使用统一时间戳作为所有AGV的时间基准
     * 2. 对error=1的故障车，全程占据时空表，确保可以生成绕行路线
     *
     * @param agvStatusList CTU状态列表
     * @return 时空占用表，key为"x,y,timeSlot"，value为CTU编号
     */
    public Map<String, String> buildSpaceTimeOccupancyMap(List<AGVStatus> agvStatusList) {
        // 重置统一时间戳为当前时间
        this.unifiedTimestamp = System.currentTimeMillis() / 1000;
        
        Map<String, String> occupancyMap = new HashMap<>();

        for (AGVStatus agv : agvStatusList) {
            if (agv.hasRemainingPath()) {
                // 处理有剩余路径的AGV
            // 处理故障车：error=1的车辆全程占据时空表
            if (agv.getError() == 1) {
                // 故障车全程占据其当前位置
                processFaultyAgvOccupancy(agv, occupancyMap);
            } else if (agv.hasRemainingPath()) {
                // 处理有剩余路径的正常AGV
                processRemainingPathOccupancy(agv, occupancyMap);
            } else if (agv.getPosition() != null && !agv.getPosition().isEmpty()) {
                // 处理静止AGV的当前位置占用
                // 处理静止的正常AGV的当前位置占用
                processStaticAgvOccupancy(agv, occupancyMap);
            }
        }

        return occupancyMap;
    }
    
    /**
     * 处理故障车的时空占用
     * 故障车(error=1)在时空表上全程占据其位置
     *
     * @param agv          故障CTU状态
     * @param occupancyMap 时空占用表
     */
    private void processFaultyAgvOccupancy(AGVStatus agv, Map<String, String> occupancyMap) {
        String position = agv.getPosition();
        if (position == null || position.isEmpty()) {
            return;
        }
        
        // 获取位置坐标
        int[] coord = JsonUtils.getCoordinate(position, pathMapping);
        if (coord == null) {
            return;
        }
        
        CTUPhysicalConfig config = agv.getPhysicalConfig();
        
        long occupancyDuration = 86400; 
        
        // 从统一时间戳开始占用
        for (long timeSlot = unifiedTimestamp; timeSlot < unifiedTimestamp + occupancyDuration; timeSlot++) {
            String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
            occupancyMap.put(spaceTimeKey, agv.getAgvId() + "_FAULT");
            
            occupyAdjacentSpaces(coord, timeSlot, agv.getAgvId() + "_FAULT", occupancyMap, config);
        }
        
        System.out.println("  故障AGV " + agv.getAgvId() + " 全程占用位置 " + position + 
                         " (坐标: " + coord[0] + "," + coord[1] + "), 时间: " + 
                         unifiedTimestamp + " ~ " + (unifiedTimestamp + occupancyDuration) + "秒");
    }
    
    /**
@@ -74,12 +120,11 @@
        }
        
        CTUPhysicalConfig config = agv.getPhysicalConfig();
        long currentTime = System.currentTimeMillis() / 1000; // 转换为秒
        
        // 静止AGV占用当前位置的长时间段（假设300秒）
        long occupancyDuration = 300; // 300秒的占用时间
        
        for (long timeSlot = currentTime; timeSlot < currentTime + occupancyDuration; timeSlot++) {

        for (long timeSlot = unifiedTimestamp; timeSlot < unifiedTimestamp + occupancyDuration; timeSlot++) {
            String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
            occupancyMap.put(spaceTimeKey, agv.getAgvId());
            
@@ -101,8 +146,11 @@
        List<PathCode> codeList = remainingPath.getCodeList();
        CTUPhysicalConfig config = agv.getPhysicalConfig();

        // 从当前位置开始计算时空占用
        long currentTime = agv.getNextPointArrivalTime();
        // 从统一时间戳或AGV的下一点到达时间开始，取较大值
        // 如果AGV已经在执行路径且下一点到达时间在未来，则使用实际时间
        // 如果AGV刚开始或时间已过，则使用统一时间戳
        long agvNextArrivalTime = agv.getNextPointArrivalTime() / 1000; // 转换为秒
        long currentTime = Math.max(unifiedTimestamp, agvNextArrivalTime) * 1000; // 转回毫秒进行计算
        int startIndex = agv.getCurrentPathIndex();

        for (int i = startIndex; i < codeList.size(); i++) {