优化更新

jianghaiyue

2025-11-05 642f20dd1b068e23146b8ec96c5eac8b63c96b87

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

@@ -170,7 +170,7 @@

        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");
@@ -299,6 +299,9 @@
        // 7.5. 最终避让检查
        performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);

        // 7.6. 重新计算所有路径的时间戳
        recalculateAllPathTimings(plannedPaths, agvStatusList, unifiedTimestamp);

        // 8. 最终碰撞检测和解决
        System.out.println("步骤6: 最终碰撞检测");
        List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);
@@ -341,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;
        }
@@ -376,8 +380,8 @@
        processedPath.setSegId(segId);

        if (timeCalculator != null && !remainingCodes.isEmpty()) {
            // 获取AGV的下一个路径点到达时间作为起始时间
            long startTime = agv.getNextPointArrivalTime();
            // 使用统一时间戳作为起始时间（转换为毫秒）
            long startTime = unifiedTimestamp * 1000;
            
            CTUPhysicalConfig config = agv.getPhysicalConfig();
            
@@ -619,7 +623,6 @@
            return;
        }

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

@@ -627,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);
            }
        }
@@ -796,6 +802,50 @@
        }
    }

    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 + "秒）");
    }

    /**
     * 识别需要让行的AGV
     * 检查空闲AGV是否占用了其他AGV剩余路径上的位置