优化更新

jianghaiyue

2025-11-05 12f30a09bce1c61b2ae90129124cdc467a59b074

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

@@ -103,33 +103,70 @@
        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. 构建现有剩余路径的时空占用表
        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) {
@@ -149,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");
@@ -167,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();
@@ -200,7 +237,7 @@

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

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

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

                plannedPaths.add(plannedPath);
@@ -225,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<>();
@@ -242,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");
@@ -252,18 +288,18 @@
                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. 最终碰撞检测和解决（针对同时生成的新路径）
        // 8. 最终碰撞检测和解决
        System.out.println("步骤6: 最终碰撞检测");
        List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);

@@ -407,27 +443,32 @@
     * @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) {
        // 尝试基本路径规划
        PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
        if (basicPath == null) {
                                                         AGVStatus agvStatus,
                                                         long unifiedTimestamp) {
        // 带时空占用表的路径规划
        long startTimeMs = unifiedTimestamp * 1000; 
        PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
                startPos, 
                endPos, 
                constraints, 
                occupancyMap, 
                agvStatus.getPhysicalConfig(),
                startTimeMs 
        );
        
        if (spacetimePath == null) {
            return null;
        }

        // 找到安全的起始时间
        long safeStartTime = remainingPathProcessor.findSafeStartTime(
                basicPath, occupancyMap, agvStatus.getPhysicalConfig()
        );
        timeCalculator.calculatePathTiming(spacetimePath, startTimeMs, agvStatus.getPhysicalConfig(), 0.0);

        // 使用统一的时间计算器设置精确的时间信息
        timeCalculator.calculatePathTiming(basicPath, safeStartTime, agvStatus.getPhysicalConfig(), 0.0);

        return basicPath;
        return spacetimePath;
    }

    /**
@@ -516,12 +557,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) {
@@ -539,7 +582,7 @@

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

                    if (idlePath != null) {
@@ -699,7 +742,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;
@@ -728,7 +772,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");
@@ -817,7 +861,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();
        
@@ -843,7 +888,7 @@
        
        // 3. 规划避让路径
        PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
            currentPos, targetPos, constraints, occupancyMap, yieldAgv
            currentPos, targetPos, constraints, occupancyMap, yieldAgv, unifiedTimestamp
        );
        
        if (yieldPath != null) {
@@ -1104,7 +1149,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<>());

@@ -1125,7 +1171,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);
            }
@@ -1155,7 +1201,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();
@@ -1171,9 +1218,9 @@
            return null;
        }

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

        if (plannedPath != null) {