更新版本

jianghaiyue

17 小时以前 d808837cd368c3772962be591aa6532bcc0cf3e4

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

@@ -4,6 +4,8 @@
import com.algo.model.ExecutingTask;
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;
import com.algo.model.CTUPhysicalConfig;
import com.algo.util.PathTimeCalculator;

import java.util.*;

@@ -17,6 +19,11 @@
     * 碰撞检测器
     */
    private final CollisionDetector collisionDetector;
    
    /**
     * 路径时间计算器
     */
    private PathTimeCalculator timeCalculator;

    /**
     * 构造函数
@@ -25,6 +32,13 @@
     */
    public CollisionResolver(CollisionDetector collisionDetector) {
        this.collisionDetector = collisionDetector;
    }
    
    /**
     * 设置时间计算器
     */
    public void setTimeCalculator(PathTimeCalculator timeCalculator) {
        this.timeCalculator = timeCalculator;
    }

    /**
@@ -243,7 +257,7 @@
    }

    /**
     * 为路径添加延迟
     * 为路径添加延迟（增强版，正确重计算时间窗）
     *
     * @param path       路径
     * @param timeStep   延迟开始的时间步
@@ -263,6 +277,9 @@

        // 获取延迟位置的路径代码
        PathCode delayCode = codeList.get(timeStep);
        
        // 计算延迟时间（每个延迟步骤1秒）
        long delayDuration = delaySteps * 1000L;

        // 创建延迟步骤
        List<PathCode> delaySteps_list = new ArrayList<>();
@@ -273,14 +290,69 @@
            waitCode.setPosType(delayCode.getPosType());
            waitCode.setLev(delayCode.getLev());
            waitCode.setTargetPoint(false); // 等待步骤不是目标点
            
            // 设置等待步骤的时间信息
            if (delayCode.getArrivalTime() != null) {
                long waitStartTime = delayCode.getArrivalTime() + (i * 1000L);
                waitCode.setArrivalTime(waitStartTime);
                waitCode.setDepartureTime(waitStartTime + 1000L);
            }
            
            delaySteps_list.add(waitCode);
        }

        // 插入延迟步骤
        codeList.addAll(timeStep, delaySteps_list);

        // 更新原路径点的时间（向后推迟）
        if (delayCode.getArrivalTime() != null) {
            delayCode.setArrivalTime(delayCode.getArrivalTime() + delayDuration);
            if (delayCode.getDepartureTime() != null) {
                delayCode.setDepartureTime(delayCode.getDepartureTime() + delayDuration);
            }
        }

        // 更新路径
        path.setCodeList(codeList);
        
        // 🔧 关键修复：重新计算后续路径点的时间窗
        if (timeCalculator != null) {
            CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
            timeCalculator.recalculatePathTimingFromIndex(path, timeStep + delaySteps, defaultConfig);
        } else {
            // 备用方案：手动更新后续时间
            updateSubsequentPathTiming(path, timeStep + delaySteps, delayDuration);
        }
    }
    
    /**
     * 备用方案：手动更新后续路径点的时间
     */
    private void updateSubsequentPathTiming(PlannedPath path, int fromIndex, long timeOffset) {
        List<PathCode> codeList = path.getCodeList();
        
        for (int i = fromIndex; i < codeList.size(); i++) {
            PathCode pathCode = codeList.get(i);
            
            if (pathCode.getArrivalTime() != null) {
                pathCode.setArrivalTime(pathCode.getArrivalTime() + timeOffset);
            }
            
            if (pathCode.getDepartureTime() != null) {
                pathCode.setDepartureTime(pathCode.getDepartureTime() + timeOffset);
            }
            
            if (pathCode.getCumulativeTime() != null) {
                pathCode.setCumulativeTime(pathCode.getCumulativeTime() + timeOffset);
            }
        }
    }
    
    /**
     * 创建默认物理配置
     */
    private CTUPhysicalConfig createDefaultPhysicalConfig() {
        return new CTUPhysicalConfig(); // 使用默认构造函数
    }

    /**