#

vincentlu

2025-05-13 ebd2f4397a92c6a5096de1b86d59154363344720

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java

@@ -15,7 +15,6 @@
import org.springframework.stereotype.Service;

import java.util.*;
import java.util.concurrent.CopyOnWriteArrayList;

/**
 * Created by vincent on 6/12/2024
@@ -29,6 +28,9 @@
    public static final boolean OPEN_TURN_COST_WEIGHT = Boolean.TRUE;

    public static final int WEIGHT_CALC_FACTOR = 1;

    // right left up down
    private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};

    @Autowired
    private MapDataDispatcher mapDataDispatcher;
@@ -48,6 +50,13 @@

        PriorityQueue<AStarNavigateNode> openQueue = new PriorityQueue<>();
        Set<AStarNavigateNode> existNodes = new HashSet<>();
        Map<String, Integer> bestGMap = new HashMap<>();

        start.setG(0);
        start.setH(calcNodeCost(start, end));
        start.setF(start.getG() + start.getH());
        String startKey = start.getX() + "_" + start.getY();
        bestGMap.put(startKey, start.getG());

        openQueue.add(start);
        existNodes.add(start);
@@ -56,15 +65,23 @@
        String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
        DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(null);
        String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(null);

        long getNeighborNodesTime = 0;
        int getNeighborNodesCount = 0;

        while (!openQueue.isEmpty()) {
            // 取优先队列顶部元素并且把这个元素从Open表中删除，取F值最小的节点
            AStarNavigateNode currentNode = openQueue.poll();

            // 终点
            if (currentNode.getX() == end.getX() && currentNode.getY() == end.getY()) {
//                System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
                return currentNode;
            }
            long currentTime = System.currentTimeMillis();
            List<AStarNavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
            getNeighborNodesTime += System.currentTimeMillis() - currentTime;
            getNeighborNodesCount ++;
            for (AStarNavigateNode node : neighbourNodes) {
                node.setCodeData(codeMatrix[node.getX()][node.getY()]);

@@ -90,6 +107,9 @@
                assert !vehicle.equals(DynamicNodeType.BLOCK.val);
                if (!vehicle.equals(DynamicNodeType.ACCESS.val)) {
                    if (!vehicle.equals(agvNo)) {
                        if (lock) {
                            continue;
                        }

                        // 如果存在车辆，则增加权重 2 或者 3，因为拐点会增加权重 1
                        // vehicle已经为当前segment做过了避让，且避让任务已完成，则权重值增加
@@ -100,10 +120,6 @@
                                weight += (WEIGHT_CALC_FACTOR * 2);
                            }
                        }

                        if (lock) {
                            continue;
                        }
                    }
                }

@@ -113,7 +129,6 @@
                if (!waveNode.equals(WaveNodeType.ENABLE.val)) {
                    List<String> waveNodeList = MapDataUtils.parseWaveNode(waveNode);
                    List<String> otherWaveList = MapDataUtils.hasOtherWave(waveNodeList, agvNo);

                    if (!Cools.isEmpty(otherWaveList)) {

                        if (lock) {
@@ -159,11 +174,19 @@
                node.setH(calcNodeCost(node, end));
                node.setF(node.getG() + node.getH());

                openQueue.add(node);
                existNodes.add(node);
                String key = node.getX() + "_" + node.getY();
                Integer recordedG = bestGMap.get(key);
                if (recordedG == null || node.getG() <= recordedG) {
                    bestGMap.put(key, node.getG());

                    openQueue.add(node);
                }

//                openQueue.add(node);
//                existNodes.add(node);
            }
        }

//        System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
        return null;
    }

@@ -171,20 +194,33 @@
    private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes) {
        int x = currentNode.getX();
        int y = currentNode.getY();
        AStarNavigateNode parent = currentNode.getParent();

        List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
//        List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
        List<AStarNavigateNode> neighbourNodes = new ArrayList<>();

        List<AStarNavigateNode> possibleNodes = Arrays.asList(
                new AStarNavigateNode(x, y + 1), // right
                new AStarNavigateNode(x, y - 1), // left
                new AStarNavigateNode(x - 1, y), // up
                new AStarNavigateNode(x + 1, y)  // down
        );
        List<AStarNavigateNode> possibleNodes = new ArrayList<>();
        for (int[] d: DIRECTIONS) {
            int nx = x + d[0];
            int ny = y + d[1];
            // 如果父节点不为空，并且 (nx,ny) 等于父节点坐标，则跳过
            if (parent != null && nx == parent.getX() && ny == parent.getY()) {
                continue;
            }
            possibleNodes.add(new AStarNavigateNode(nx, ny));
        }

        possibleNodes.parallelStream()
                .map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
                .filter(Objects::nonNull)
                .forEach(neighbourNodes::add);
//        possibleNodes.parallelStream()
//                .map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
//                .filter(Objects::nonNull)
//                .forEach(neighbourNodes::add);

        for (AStarNavigateNode pn : possibleNodes) {
            AStarNavigateNode next = extendNeighborNodes(currentNode, pn, mapMatrix, existNodes, null, null);
            if (next != null) {
                neighbourNodes.add(next);
            }
        }

        return neighbourNodes;
    }
@@ -215,9 +251,9 @@

        assert mapMatrix[x][y] == MapNodeType.ENABLE.val;

        if (existNodes.contains(nextNode)) {
            return null;
        }
//        if (existNodes.contains(nextNode)) {
//            return null;
//        }

        // 判断通过性
        String routeCdaKey = RouteGenerator.generateRouteCdaKey(new int[]{currentNode.getX(), currentNode.getY()}, new int[]{nextNode.getX(), nextNode.getY()});