优化更新

jianghaiyue

6 天以前 1d520bffcd63cb8389c9cdf719c3cf7e7c4af567

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

@@ -1,26 +1,24 @@
package com.algo.service;

import com.algo.config.EnvDataConfig;
import com.algo.model.*;
import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

import javax.annotation.PostConstruct;
import java.util.*;
import java.util.concurrent.*;

/**
 * 路径规划服务
 */
@Service
public class PathPlanningService {

    /**
     * 路径映射表
     */
    private Map<String, Map<String, Integer>> pathMapping;

    /**
     * 环境配置
     */
    private Map<String, Object> environmentConfig;
    @Autowired
    private EnvDataConfig envDataConfig;

    /**
     * 执行中任务提取
@@ -46,64 +44,48 @@
     * 剩余路径处理
     */
    private RemainingPathProcessor remainingPathProcessor;

    
    /**
     * 线程池大小
     * 路径时间计算器
     */
    private final int threadPoolSize;
    private PathTimeCalculator timeCalculator;


    /**
     * 线程池
     */
    private final ExecutorService executorService;
    private final ExecutorService executorService = Executors.newFixedThreadPool(Math.max(4, Runtime.getRuntime().availableProcessors()));

    /**
     * CTU批处理大小
     */
    private final int batchSize = 10;

    /**
     * 构造函数
     *
     * @param pathMapping       路径映射表
     * @param environmentConfig 环境配置
     * @param taskDataList      任务数据列表
     */
    public PathPlanningService(Map<String, Map<String, Integer>> pathMapping,
                               Map<String, Object> environmentConfig,
                               List<TaskData> taskDataList) {
        this.pathMapping = pathMapping;
        this.environmentConfig = environmentConfig;

        this.threadPoolSize = Math.max(4, Runtime.getRuntime().availableProcessors());
        this.executorService = Executors.newFixedThreadPool(threadPoolSize);

        // 初始化
        initializeComponents(taskDataList);

        System.out.println("路径规划服务初始化完成（线程池大小: " + threadPoolSize + "）");
    }

    /**
     * 初始化各个组件
     *
     * @param taskDataList 任务数据列表
     */
    private void initializeComponents(List<TaskData> taskDataList) {
        // 初始化任务提取器
        this.taskExtractor = new ExecutingTaskExtractor(pathMapping, taskDataList);
    @PostConstruct
    public void initializeComponents() {

        // 初始化路径规划器
        this.pathPlanner = new AStarPathPlanner(pathMapping);
        this.pathPlanner = new AStarPathPlanner(envDataConfig.getPathMapping());

        // 初始化碰撞检测器
        this.collisionDetector = new CollisionDetector(pathMapping);
        this.collisionDetector = new CollisionDetector(envDataConfig.getPathMapping());

        // 初始化碰撞解决器
        this.collisionResolver = new CollisionResolver(collisionDetector);

        // 初始化剩余路径处理器
        this.remainingPathProcessor = new RemainingPathProcessor(pathMapping);
        this.remainingPathProcessor = new RemainingPathProcessor(envDataConfig.getPathMapping());
        
        // 初始化时间计算器
        Map<String, double[]> realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
        this.timeCalculator = new PathTimeCalculator(envDataConfig.getPathMapping(), realCoordinateMapping);
        
        // 为碰撞解决器设置时间计算器
        this.collisionResolver.setTimeCalculator(timeCalculator);
    }

    /**
@@ -114,9 +96,12 @@
     * @param constraints    路径约束条件
     * @return 路径规划结果
     */
    public PathPlanningResult planAllAgvPaths(List<AGVStatus> agvStatusList,
    public PathPlanningResult planAllAgvPaths(List<TaskData> taskList, List<AGVStatus> agvStatusList,
                                              boolean includeIdleAgv,
                                              List<double[]> constraints) {
        // 初始化任务提取器
        this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);

        long startTime = System.currentTimeMillis();

        System.out.println("开始为 " + agvStatusList.size() + " 个CTU规划");
@@ -162,6 +147,24 @@
                System.out.println("CTU " + agv.getAgvId() + " 剩余路径处理完成，路径长度: " +
                        remainingPath.getCodeList().size());
            }
        }

        // 3.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);
                if (yieldPath != null) {
                    plannedPaths.add(yieldPath);
                    plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
                    System.out.println("  AGV " + yieldAgv.getAgvId() + " 让行路径规划成功，从 " + 
                        yieldAgv.getPosition() + " 移开");
                }
            }
        } else {
            System.out.println("  无需让行的AGV");
        }

        // 4. 为需要新路径的CTU提取执行中任务
@@ -284,17 +287,51 @@
        PlannedPath remainingPath = agv.getRemainingPath();
        List<PathCode> remainingCodes = new ArrayList<>();

        // 从当前位置开始，获取剩余路径
        // 获取剩余路径
        List<PathCode> originalCodes = remainingPath.getCodeList();
        for (int i = agv.getCurrentPathIndex(); i < originalCodes.size(); i++) {
            remainingCodes.add(originalCodes.get(i));
            PathCode originalCode = originalCodes.get(i);
            PathCode newCode = new PathCode(originalCode.getCode(), originalCode.getDirection());
            newCode.setActionType(originalCode.getActionType());
            newCode.setTaskId(originalCode.getTaskId());
            newCode.setPosType(originalCode.getPosType());
            newCode.setLev(originalCode.getLev());
            newCode.setTargetPoint(originalCode.isTargetPoint());
            remainingCodes.add(newCode);
        }

        // 创建新的路径对象
        PlannedPath processedPath = new PlannedPath();
        processedPath.setAgvId(agv.getAgvId());
        processedPath.setCodeList(remainingCodes);
        processedPath.setSegId(agv.getAgvId() + "_REMAINING_" + System.currentTimeMillis());
        
        // 使用输入中的segId，如果没有则生成新的
        String segId = remainingPath.getSegId();
        if (segId == null || segId.trim().isEmpty()) {
            segId = agv.getAgvId() + "_REMAINING_" + System.currentTimeMillis();
        }
        processedPath.setSegId(segId);

        if (timeCalculator != null && !remainingCodes.isEmpty()) {
            // 获取AGV的下一个路径点到达时间作为起始时间
            long startTime = agv.getNextPointArrivalTime();
            
            CTUPhysicalConfig config = agv.getPhysicalConfig();
            
            // 估算当前速度（AGV移动中为正常速度；静止为0）
            double initialSpeed = agv.hasRemainingPath() && agv.getRemainingPathLength() > 0 
                                ? config.getNormalSpeed() : 0.0;
            
            // 计算时间信息
            // arrivalTime, departureTime, cumulativeTime
            timeCalculator.calculatePathTiming(
                processedPath, 
                startTime, 
                config, 
                initialSpeed
            ); 
        } else {
            System.out.println("  未能为剩余路径设置时间信息 - AGV: " + agv.getAgvId());
        }

        return processedPath;
    }
@@ -348,7 +385,7 @@
                                                         List<double[]> constraints,
                                                         Map<String, String> occupancyMap,
                                                         AGVStatus agvStatus) {
        // 首先尝试基本路径规划
        // 尝试基本路径规划
        PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
        if (basicPath == null) {
            return null;
@@ -359,8 +396,8 @@
                basicPath, occupancyMap, agvStatus.getPhysicalConfig()
        );

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

        return basicPath;
    }
@@ -392,9 +429,12 @@

                // 如果有方向变化，增加转向时间
                PathCode nextCode = codeList.get(i + 1);
                if (!pathCode.getDirection().equals(nextCode.getDirection())) {
                    double turnTime = config.getTurnTime(pathCode.getDirection(), nextCode.getDirection());
                    currentTime += (long) (turnTime * 1000);
                // 添加null检查
                if (pathCode.getDirection() != null && nextCode.getDirection() != null) {
                    if (!pathCode.getDirection().equals(nextCode.getDirection())) {
                        double turnTime = config.getTurnTime(pathCode.getDirection(), nextCode.getDirection());
                        currentTime += (long) (turnTime * 1000);
                    }
                }
            }
        }
@@ -416,7 +456,7 @@
        long currentTime = System.currentTimeMillis() / 1000; // 转换为秒

        for (PathCode pathCode : codeList) {
            int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), pathMapping);
            int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), envDataConfig.getPathMapping());
            if (coord != null) {
                String spaceTimeKey = coord[0] + "," + coord[1] + "," + currentTime;
                occupancyMap.put(spaceTimeKey, agvStatus.getAgvId());
@@ -625,6 +665,174 @@
    }

    /**
     * 识别需要让行的AGV
     * 检查空闲AGV是否占用了其他AGV剩余路径上的位置
     *
     * @param agvStatusList 所有AGV状态列表
     * @param plannedPaths  已规划的路径列表（包含剩余路径）
     * @return 需要让行的AGV列表
     */
    private List<AGVStatus> identifyYieldingAgvs(List<AGVStatus> agvStatusList, List<PlannedPath> plannedPaths) {
        List<AGVStatus> yieldingAgvs = new ArrayList<>();
        
        // 收集所有已规划路径上的位置
        Set<String> occupiedPositions = new HashSet<>();
        for (PlannedPath path : plannedPaths) {
            if (path.getCodeList() != null) {
                for (PathCode code : path.getCodeList()) {
                    if (code.getCode() != null) {
                        occupiedPositions.add(code.getCode());
                    }
                }
            }
        }
        
        // 检查每个AGV
        for (AGVStatus agv : agvStatusList) {
            // 如果这个AGV已经有剩余路径，跳过（已在任务）
            if (agv.hasRemainingPath()) {
                continue;
            }
            
            // 检查AGV是否有位置信息
            if (agv.getPosition() == null || agv.getPosition().trim().isEmpty()) {
                continue;
            }
            
            // 检查AGV是否是空闲状态（status=0）
            // 只要没有剩余路径且有位置，就检查是否需要让行
            int status = agv.getStatus();
            if (status != 0) {
                // status=1或2（问题或忙碌）等其他状态跳过
                continue;
            }
            
            // 检查该AGV的位置是否在其他AGV的路径上
            String currentPos = agv.getPosition();
            if (occupiedPositions.contains(currentPos)) {
                System.out.println(" CTU " + agv.getAgvId() + 
                    " (status=" + status + ") 在位置 " + currentPos + " 占用了其他CTU路径，需要让行");
                yieldingAgvs.add(agv);
            }
        }
        
        return yieldingAgvs;
    }

    /**
     * 为让行AGV规划避让路径
     *
     * @param yieldAgv         需要让行的AGV
     * @param existingPaths    已存在的路径列表
     * @param occupancyMap     时空占用表
     * @param constraints      路径约束
     * @return 让行路径
     */
    private PlannedPath planYieldPath(AGVStatus yieldAgv, List<PlannedPath> existingPaths,
                                     Map<String, String> occupancyMap, List<double[]> constraints) {
        String currentPos = yieldAgv.getPosition();
        String agvId = yieldAgv.getAgvId();
        
        // 1. 找到安全的目标位置
        Set<String> blockedPositions = new HashSet<>();
        for (PlannedPath path : existingPaths) {
            if (path.getCodeList() != null) {
                for (PathCode code : path.getCodeList()) {
                    if (code.getCode() != null) {
                        blockedPositions.add(code.getCode());
                    }
                }
            }
        }
        
        // 2. 寻找合适让行目标位置
        String targetPos = findYieldTargetPosition(currentPos, blockedPositions, yieldAgv);
        
        if (targetPos == null || targetPos.equals(currentPos)) {
            System.out.println("    AGV " + agvId + " 无法找到合适的让行位置");
            return null;
        }
        
        // 3. 规划避让路径
        PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
            currentPos, targetPos, constraints, occupancyMap, yieldAgv
        );
        
        if (yieldPath != null) {
            yieldPath.setAgvId(agvId);
            yieldPath.setSegId(generateSegId("AVOID", agvId, "avoiding"));
            
            // 设置路径代码信息
            enhancePathCodesForYielding(yieldPath);
            
            // 更新占用表
            updateSpaceTimeOccupancyMap(yieldPath, occupancyMap, yieldAgv);
        }
        
        return yieldPath;
    }

    /**
     * 寻找让行的目标位置
     */
    private String findYieldTargetPosition(String currentPos, Set<String> blockedPositions, AGVStatus agv) {
        // 使用BFS搜索最近的空闲位置
        final int MAX_SEARCH_DEPTH = 10;
        
        Queue<String> queue = new LinkedList<>();
        Map<String, Integer> visited = new HashMap<>(); // 记录位置和距离
        
        queue.offer(currentPos);
        visited.put(currentPos, 0);
        
        while (!queue.isEmpty()) {
            String pos = queue.poll();
            int depth = visited.get(pos);

            if (depth >= MAX_SEARCH_DEPTH) {
                break;
            }

            List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
            for (Map<String, String> neighbor : neighbors) {
                String neighborPos = neighbor.get("code");
                
                if (neighborPos == null || visited.containsKey(neighborPos)) {
                    continue;
                }
                
                // 找到一个未被占用的位置
                if (!blockedPositions.contains(neighborPos)) {
                    System.out.println("  找到避让位置: " + neighborPos + " (距离=" + (depth + 1) + "步)");
                    return neighborPos;
                }
                
                // 加入队列继续搜索
                queue.offer(neighborPos);
                visited.put(neighborPos, depth + 1);
            }
        }
        
        System.out.println("  未找到合适的避让位置 ");
        return null;
    }

    private void enhancePathCodesForYielding(PlannedPath yieldPath) {
        if (yieldPath == null || yieldPath.getCodeList() == null) {
            return;
        }
        
        List<PathCode> codes = yieldPath.getCodeList();
        for (int i = 0; i < codes.size(); i++) {
            PathCode code = codes.get(i);
            code.setActionType("0"); 
            code.setPosType(null);
            code.setLev(0);
            code.setTargetPoint(i == codes.size() - 1); 
        }
    }

    /**
     * 路径规划结果类
     */
    public static class PathPlanningResult {