优化更新

jianghaiyue

2025-11-04 434e0ff84a7df875f223e9118f244e04f28310ca

 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;
    }

    /**