优化更新

jianghaiyue

7 天以前 4618bbf913912f85ee197ccd10644df29ad2f287

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

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

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

        System.out.println("开始为 " + agvStatusList.size() + " 个CTU规划");
        System.out.println("统一时间基准: " + unifiedTimestamp + "秒 (" + new Date(startTime) + ")");
        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)，将在时空表中占据位置");
            System.out.println("检测到 " + faultyCount + " 个故障车(error)，将在时空表中占据位置");
        }

        // 1. 构建现有剩余路径的时空占用表
        System.out.println("步骤1: 构建时空占用表");
        Map<String, String> spaceTimeOccupancyMap = remainingPathProcessor.buildSpaceTimeOccupancyMap(agvStatusList);
        Map<String, String> spaceTimeOccupancyMap = remainingPathProcessor.buildSpaceTimeOccupancyMap(agvStatusList, unifiedTimestamp);
        System.out.println("时空占用表构建完成，包含 " + spaceTimeOccupancyMap.size() + " 个占用点");

        // 2. 分类处理CTU：有剩余路径的和需要新路径的
        // 2. 初始化规划结果集合
        List<PlannedPath> plannedPaths = new ArrayList<>();
        List<ExecutingTask> executingTasks = new ArrayList<>();
        Map<String, String> plannedAgvIds = new HashMap<>(); // 提前初始化标记故障车

        // 3. 分类处理CTU：有剩余路径的和需要新路径的
        List<AGVStatus> ctuWithRemainingPaths = new ArrayList<>();
        List<AGVStatus> ctuNeedingNewPaths = new ArrayList<>();
        List<AGVStatus> unhandledAgvs = new ArrayList<>(); // 未处理的AGV

        for (AGVStatus agv : agvStatusList) {
            // 故障车已在时空占用表中处理，标记为已处理但不规划路径
            if (agv.getError() == 1) {
                plannedAgvIds.put(agv.getAgvId(), "FAULTY");
                continue;
            }
            
            if (agv.hasRemainingPath()) {
                ctuWithRemainingPaths.add(agv);
            } else if (agv.canAcceptNewTask() || (includeIdleAgv && isAgvIdle(agv))) {
            } else if (agv.canAcceptNewTask()) {
                // 可以接受新任务的AGV（有任务或可以接受新任务）
                ctuNeedingNewPaths.add(agv);
            } else if (includeIdleAgv && isAgvIdle(agv)) {
                // 完全空闲的AGV（无任务，无剩余路径）
                ctuNeedingNewPaths.add(agv);
            } else {
                // 其他情况：不可用、有剩余路径但>1、有任务但不可用等
                unhandledAgvs.add(agv);
            }
        }

        System.out.println("CTU分类完成: 有剩余路径=" + ctuWithRemainingPaths.size() +
                ", 需要新路径=" + ctuNeedingNewPaths.size());
                ", 需要新路径=" + ctuNeedingNewPaths.size() +
                ", 未处理=" + unhandledAgvs.size());
        
        // 记录未处理的AGV信息（debug）
        if (!unhandledAgvs.isEmpty()) {
            for (AGVStatus agv : unhandledAgvs) {
                System.out.println("  - AGV " + agv.getAgvId() + 
                    ": error=" + agv.getError() + 
                    ", hasRemainingPath=" + agv.hasRemainingPath() + 
                    ", canAcceptNewTask=" + agv.canAcceptNewTask() + 
                    ", isIdle=" + isAgvIdle(agv) +
                    ", isAvailable=" + agv.isAvailable());
            }
        }

        // 3. 处理有剩余路径的CTU
        List<PlannedPath> plannedPaths = new ArrayList<>();
        List<ExecutingTask> executingTasks = new ArrayList<>();
        Map<String, String> plannedAgvIds = new HashMap<>();
        // 4. 处理有剩余路径的CTU

        System.out.println("步骤2: 处理有剩余路径的CTU");
        for (AGVStatus agv : ctuWithRemainingPaths) {
            PlannedPath remainingPath = processRemainingPath(agv);
            PlannedPath remainingPath = processRemainingPath(agv, unifiedTimestamp);
            if (remainingPath != null) {
                plannedPaths.add(remainingPath);
                plannedAgvIds.put(agv.getAgvId(), "REMAINING_PATH");
@@ -157,13 +186,13 @@
            }
        }

        // 3.5. 检查空闲AGV是否需要让行
        // 4.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);
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                if (yieldPath != null) {
                    plannedPaths.add(yieldPath);
                    plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -175,12 +204,12 @@
            System.out.println("  无需让行的AGV");
        }

        // 4. 为需要新路径的CTU提取执行中任务
        // 5. 为需要新路径的CTU提取执行中任务
        System.out.println("步骤3: 提取需要新路径的CTU任务");
        List<ExecutingTask> newTasks = taskExtractor.extractExecutingTasks(ctuNeedingNewPaths);
        System.out.println("提取到 " + newTasks.size() + " 个新任务");

        // 5. 为新任务规划路径（考虑时空约束）
        // 6. 为新任务规划路径（考虑时空约束）
        System.out.println("步骤4: 为新任务规划路径");
        for (ExecutingTask task : newTasks) {
            String agvId = task.getAgvId();
@@ -208,7 +237,7 @@

            // 规划时空安全的路径
            PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
                    currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
                    currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
            );

            if (plannedPath != null) {
@@ -216,7 +245,6 @@
                plannedPath.setAgvId(agvId);
                plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));

                // 增强路径代码信息
                enhancePathCodes(plannedPath, task);

                plannedPaths.add(plannedPath);
@@ -233,11 +261,11 @@
            }
        }

        // 6. 处理空闲CTU
        // 7. 处理空闲CTU
        if (includeIdleAgv) {
            System.out.println("步骤5: 处理空闲CTU");
            
            // 6.1. 检查空闲AGV是否占用已规划路径，如果占用则生成避让路径
            // 7.1. 检查空闲AGV是否占用已规划路径，如果占用则生成避让路径
            List<AGVStatus> yieldingIdleAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);

            List<AGVStatus> yieldingAgvsToHandle = new ArrayList<>();
@@ -250,7 +278,7 @@
            if (!yieldingAgvsToHandle.isEmpty()) {
                System.out.println( yieldingAgvsToHandle.size() + " 个需要避让的空闲AGV");
                for (AGVStatus yieldAgv : yieldingAgvsToHandle) {
                    PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
                    PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                    if (yieldPath != null) {
                        plannedPaths.add(yieldPath);
                        plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -260,18 +288,21 @@
                System.out.println("  无需避让的空闲AGV");
            }
            
            // 6.2. 然后为剩余的空闲AGV规划待机/充电路径
            // 7.2. 然后为剩余的空闲AGV规划待机/充电路径
            List<PlannedPath> idlePaths = planIdleAgvPathsWithConstraints(
                    ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap
                    ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap, unifiedTimestamp
            );
            plannedPaths.addAll(idlePaths);
            System.out.println("  空闲CTU路径规划，数量: " + idlePaths.size());
        }

        // 6.5. 最终避让检查
        performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints);
        // 7.5. 最终避让检查
        performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);

        // 7. 最终碰撞检测和解决（针对同时生成的新路径）
        // 7.6. 重新计算所有路径的时间戳
        recalculateAllPathTimings(plannedPaths, agvStatusList, unifiedTimestamp);

        // 8. 最终碰撞检测和解决
        System.out.println("步骤6: 最终碰撞检测");
        List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);

@@ -313,9 +344,10 @@
     * 处理CTU的剩余路径
     *
     * @param agv CTU状态
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 处理后的路径
     */
    private PlannedPath processRemainingPath(AGVStatus agv) {
    private PlannedPath processRemainingPath(AGVStatus agv, long unifiedTimestamp) {
        if (!agv.hasRemainingPath()) {
            return null;
        }
@@ -348,8 +380,8 @@
        processedPath.setSegId(segId);

        if (timeCalculator != null && !remainingCodes.isEmpty()) {
            // 获取AGV的下一个路径点到达时间作为起始时间
            long startTime = agv.getNextPointArrivalTime();
            // 使用统一时间戳作为起始时间（转换为毫秒）
            long startTime = unifiedTimestamp * 1000;
            
            CTUPhysicalConfig config = agv.getPhysicalConfig();
            
@@ -415,27 +447,30 @@
     * @param constraints  约束条件
     * @param occupancyMap 时空占用表
     * @param agvStatus    CTU状态
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 规划的路径
     */
    private PlannedPath planPathWithSpaceTimeConstraints(String startPos, String endPos,
                                                         List<double[]> constraints,
                                                         Map<String, String> occupancyMap,
                                                         AGVStatus agvStatus) {
                                                         AGVStatus agvStatus,
                                                         long unifiedTimestamp) {
        // 带时空占用表的路径规划
        long startTimeMs = unifiedTimestamp * 1000; 
        PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
                startPos, 
                endPos, 
                constraints, 
                occupancyMap, 
                agvStatus.getPhysicalConfig()
                agvStatus.getPhysicalConfig(),
                startTimeMs 
        );
        
        if (spacetimePath == null) {
            return null;
        }

        long startTime = System.currentTimeMillis();
        timeCalculator.calculatePathTiming(spacetimePath, startTime, agvStatus.getPhysicalConfig(), 0.0);
        timeCalculator.calculatePathTiming(spacetimePath, startTimeMs, agvStatus.getPhysicalConfig(), 0.0);

        return spacetimePath;
    }
@@ -526,12 +561,14 @@
     * @param plannedAgvIds 已规划CTU映射
     * @param constraints   约束条件
     * @param occupancyMap  时空占用表
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 空闲CTU路径列表
     */
    private List<PlannedPath> planIdleAgvPathsWithConstraints(List<AGVStatus> agvStatusList,
                                                              Map<String, String> plannedAgvIds,
                                                              List<double[]> constraints,
                                                              Map<String, String> occupancyMap) {
                                                              Map<String, String> occupancyMap,
                                                              long unifiedTimestamp) {
        List<PlannedPath> idlePaths = new ArrayList<>();

        for (AGVStatus agvStatus : agvStatusList) {
@@ -549,7 +586,7 @@

                if (targetPos != null && !targetPos.equals(currentPos)) {
                    PlannedPath idlePath = planPathWithSpaceTimeConstraints(
                            currentPos, targetPos, constraints, occupancyMap, agvStatus
                            currentPos, targetPos, constraints, occupancyMap, agvStatus, unifiedTimestamp
                    );

                    if (idlePath != null) {
@@ -586,7 +623,6 @@
            return;
        }

        String actionType = "2"; // 任务类型
        String taskId = task.getTaskId();
        int backpackLevel = task.getBackpackIndex();

@@ -594,20 +630,23 @@
            PathCode pathCode = codeList.get(i);

            // 设置基本信息
            pathCode.setActionType(actionType);
            pathCode.setTaskId(taskId);
            pathCode.setLev(backpackLevel);

            // 设置目标点信息
            if (i == codeList.size() - 1) { // 最后一个点
                pathCode.setTargetPoint(true);
                // 根据任务类型设置actionType和posType（
                if (task.isLoaded()) {
                    pathCode.setActionType("2"); // 放货
                    pathCode.setPosType("2"); // 放货
                } else {
                    pathCode.setActionType("1"); // 取货
                    pathCode.setPosType("1"); // 取货
                }
            } else {
                pathCode.setTargetPoint(false);
                pathCode.setActionType(null); 
                pathCode.setPosType(null);
            }
        }
@@ -709,7 +748,8 @@
                                           List<PlannedPath> plannedPaths,
                                           Map<String, String> plannedAgvIds,
                                           Map<String, String> spaceTimeOccupancyMap,
                                           List<double[]> constraints) {
                                           List<double[]> constraints,
                                           long unifiedTimestamp) {
        final int MAX_ITERATIONS = 5; // 最大迭代次数
        int iteration = 0;
        int totalYieldingCount = 0;
@@ -738,7 +778,7 @@
                    continue;
                }
                
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                if (yieldPath != null) {
                    plannedPaths.add(yieldPath);
                    plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -760,6 +800,50 @@
        if (totalYieldingCount > 0) {
            System.out.println("  避让检查完成：共处理 " + totalYieldingCount + " 个避让AGV，迭代 " + iteration + " 轮");
        }
    }

    private void recalculateAllPathTimings(List<PlannedPath> plannedPaths, 
                                           List<AGVStatus> agvStatusList, 
                                           long unifiedTimestamp) {
        if (plannedPaths == null || plannedPaths.isEmpty() || timeCalculator == null) {
            return;
        }

        Map<String, AGVStatus> agvStatusMap = new HashMap<>();
        for (AGVStatus agv : agvStatusList) {
            if (agv.getAgvId() != null) {
                agvStatusMap.put(agv.getAgvId(), agv);
            }
        }

        long startTimeMs = unifiedTimestamp * 1000;
        int recalculatedCount = 0;

        // 遍历所有路径，重新计算时间戳
        for (PlannedPath path : plannedPaths) {
            if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
                continue;
            }

            String agvId = path.getAgvId();
            if (agvId == null) {
                continue;
            }

            // 获取AGV的物理配置
            AGVStatus agvStatus = agvStatusMap.get(agvId);
            CTUPhysicalConfig config = (agvStatus != null && agvStatus.getPhysicalConfig() != null) 
                    ? agvStatus.getPhysicalConfig() 
                    : new CTUPhysicalConfig(); 

            double initialSpeed = 0.0;

            // 重新计算路径时间戳
            timeCalculator.calculatePathTiming(path, startTimeMs, config, initialSpeed);
            recalculatedCount++;
        }

        System.out.println("  已重新计算 " + recalculatedCount + " 条路径的时间戳（统一时间基准: " + unifiedTimestamp + "秒）");
    }

    /**
@@ -827,7 +911,8 @@
     * @return 让行路径
     */
    private PlannedPath planYieldPath(AGVStatus yieldAgv, List<PlannedPath> existingPaths,
                                     Map<String, String> occupancyMap, List<double[]> constraints) {
                                     Map<String, String> occupancyMap, List<double[]> constraints,
                                     long unifiedTimestamp) {
        String currentPos = yieldAgv.getPosition();
        String agvId = yieldAgv.getAgvId();
        
@@ -853,7 +938,7 @@
        
        // 3. 规划避让路径
        PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
            currentPos, targetPos, constraints, occupancyMap, yieldAgv
            currentPos, targetPos, constraints, occupancyMap, yieldAgv, unifiedTimestamp
        );
        
        if (yieldPath != null) {
@@ -1114,7 +1199,8 @@
    private List<PlannedPath> planTasksInParallel(List<ExecutingTask> tasks,
                                                  List<double[]> constraints,
                                                  Map<String, String> spaceTimeOccupancyMap,
                                                  List<AGVStatus> agvStatusList) {
                                                  List<AGVStatus> agvStatusList,
                                                  long unifiedTimestamp) {

        List<PlannedPath> allPaths = Collections.synchronizedList(new ArrayList<>());

@@ -1135,7 +1221,7 @@

            for (ExecutingTask task : batch) {
                Future<PlannedPath> future = executorService.submit(() -> {
                    return planSingleTaskPath(task, constraints, spaceTimeOccupancyMap, agvStatusList);
                    return planSingleTaskPath(task, constraints, spaceTimeOccupancyMap, agvStatusList, unifiedTimestamp);
                });
                futures.add(future);
            }
@@ -1165,7 +1251,8 @@
    private PlannedPath planSingleTaskPath(ExecutingTask task,
                                           List<double[]> constraints,
                                           Map<String, String> spaceTimeOccupancyMap,
                                           List<AGVStatus> agvStatusList) {
                                           List<AGVStatus> agvStatusList,
                                           long unifiedTimestamp) {

        String agvId = task.getAgvId();
        String currentPos = task.getCurrentPosition();
@@ -1181,9 +1268,9 @@
            return null;
        }

        // 规划时空安全的路径
        // 规划时空安全的路径（使用统一时间戳）
        PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
                currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
                currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
        );

        if (plannedPath != null) {