新增充电桩

zhang

2025-05-20 1313906bb1eb983d3beece810035e7fc28d6a92f

 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;
@@ -63,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()]);

@@ -97,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做过了避让，且避让任务已完成，则权重值增加
@@ -107,10 +120,6 @@
                                weight += (WEIGHT_CALC_FACTOR * 2);
                            }
                        }

                        if (lock) {
                            continue;
                        }
                    }
                }

@@ -120,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) {
@@ -178,12 +186,9 @@
//                existNodes.add(node);
            }
        }

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

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

    // 获取四周节点
    private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes) {
@@ -191,14 +196,8 @@
        int y = currentNode.getY();
        AStarNavigateNode parent = currentNode.getParent();

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

//        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> neighbourNodes = new CopyOnWriteArrayList<>();
        List<AStarNavigateNode> neighbourNodes = new ArrayList<>();

        List<AStarNavigateNode> possibleNodes = new ArrayList<>();
        for (int[] d: DIRECTIONS) {
@@ -211,10 +210,17 @@
            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;
    }