#

luxiaotao1123

2025-01-08 aa6f20d98b5d8e18ae56f9562a78d403a5417b48

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

@@ -1,11 +1,13 @@
package com.zy.acs.manager.core.service;

import com.alibaba.fastjson.JSONObject;
import com.zy.acs.common.enums.AgvDirectionType;
import com.zy.acs.framework.common.Cools;
import com.zy.acs.manager.common.utils.MapDataUtils;
import com.zy.acs.manager.core.constant.MapDataConstant;
import com.zy.acs.manager.core.domain.SortCodeDto;
import com.zy.acs.manager.core.domain.UnlockPathTask;
import com.zy.acs.manager.core.service.astart.*;
import com.zy.acs.manager.core.service.astart.domain.AStarNavigateNode;
import com.zy.acs.manager.core.service.astart.domain.DynamicNode;
import com.zy.acs.manager.core.service.floyd.FloydNavigateService;
import com.zy.acs.manager.manager.entity.Code;
@@ -15,13 +17,16 @@
import com.zy.acs.manager.manager.service.CodeService;
import com.zy.acs.manager.system.service.ConfigService;
import lombok.extern.slf4j.Slf4j;
import org.apache.commons.lang.time.StopWatch;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.stereotype.Component;

import java.util.*;
import java.util.concurrent.ConcurrentLinkedQueue;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.concurrent.TimeUnit;
import java.util.stream.Collectors;
import java.util.stream.IntStream;

/**
 * Created by vincent on 2023/6/14
@@ -44,6 +49,8 @@
    private ConfigService configService;
    @Autowired
    private ActionService actionService;
    @Autowired
    private LinkedBlockingQueue<UnlockPathTask> unlockTaskQueue;

    /**
     * 寻址 ===>> A Star
@@ -54,35 +61,26 @@
        int[] startMapIdx = mapDataDispatcher.getCodeMatrixIdx(null, startCode.getData());
        int[] endMapIdx = mapDataDispatcher.getCodeMatrixIdx(null, endCode.getData());

        NavigateNode startNode = new NavigateNode(startMapIdx[0], startMapIdx[1], startCode.getData());
        NavigateNode endNode = new NavigateNode(endMapIdx[0], endMapIdx[1], endCode.getData());
        AStarNavigateNode startNode = new AStarNavigateNode(startMapIdx[0], startMapIdx[1], startCode.getData());
        AStarNavigateNode endNode = new AStarNavigateNode(endMapIdx[0], endMapIdx[1], endCode.getData());

        long startTime = System.currentTimeMillis();
        NavigateNode finishNode = aStarNavigateService.execute(agvNo, startNode, endNode, lock, blackList, segment);
//        System.out.println("AStart spend time: " + (System.currentTimeMillis() - startTime));

//        long startTime = System.currentTimeMillis();
        AStarNavigateNode finishNode = aStarNavigateService.execute(agvNo, startNode, endNode, lock, blackList, segment);
//        System.out.println("A Star Spend time :" + (System.currentTimeMillis() - startTime));
        if (null == finishNode) {
            return new ArrayList<>();
        }

        ArrayList<NavigateNode> navigateNodes = new ArrayList<>();
        ArrayList<AStarNavigateNode> navigateNodes = new ArrayList<>();

        // 渲染
        NavigateNode parentNode = null; //  当前循环上一节点，用于拐点计算
        while (finishNode != null) {
            navigateNodes.add(finishNode);

            parentNode = finishNode;
            finishNode = finishNode.getParent();
        }

        Collections.reverse(navigateNodes);

//        for (NavigateNode navigateNode : navigateNodes) {
//            navigateNode.setParent(null);
//        }

        List<String> navigatePath = navigateNodes.stream().map(NavigateNode::getCodeData).collect(Collectors.toList());
        List<String> navigatePath = navigateNodes.stream().map(AStarNavigateNode::getCodeData).collect(Collectors.toList());

        // max count of steps
        if (navigatePath.size() > MapDataConstant.MAX_STEPS_SINGLE) {
@@ -126,7 +124,7 @@


    // 角度计算
    public Double calculateDirection(Code startCode, Code endCode) {
    public Double calculateDirection(Code startCode, Code endCode, int angleOffsetVal) {
        Double x0 = startCode.getX();
        Double y0 = startCode.getY();

@@ -136,7 +134,6 @@
        double deltaX = x1 - x0;
        double deltaY = y1 - y0;
        double angle = -Math.atan2(deltaX, deltaY);
        int angleOffsetVal = configService.getVal("mapAngleOffsetVal", Integer.class);
        angle = Math.toDegrees(angle) + angleOffsetVal;
        angle = (angle + 360) % 360; // 将角度转换为正值

@@ -144,8 +141,7 @@
    }

    // 坐标货架阈值 todo:luxiaotao
    public AgvDirectionType calculateAgvWorkDirection(JSONObject storeDirection, Loc loc, Code code) {
        storeDirection.getString("key");
    public AgvDirectionType calculateAgvWorkDirection(Loc loc, Code code) {
        Integer compDirect = loc.getCompDirect();
        AgvDirectionType agvDirectionType = null;
        if (compDirect == 0) {
@@ -181,61 +177,16 @@
    }

    public void lockPath(Integer lev, List<String> pathList, String agvNo) {
        mapDataDispatcher.modifyDynamicMatrix(lev, pathList, agvNo);
        List<int[]> codeMatrixIdxList = mapDataDispatcher.getCodeMatrixIdxList(lev, pathList);
        mapDataDispatcher.modifyDynamicMatrix(lev, codeMatrixIdxList, agvNo);
    }

    public synchronized void unlockPath(String agvNo, String codeData) {
    public void unlockPath(String agvNo, String codeData) {
        try {
            StopWatch stopWatch = new StopWatch();
            stopWatch.start();

            if (Cools.isEmpty(agvNo, codeData)) {
                return;
            }

            Integer lev = null;

            String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(null);
            int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, codeData);


            DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);

            DynamicNode dynamicNode = dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];


            Integer serial = dynamicNode.getSerial();

            List<String> resetCodeList = new ArrayList<>();

            for (int i = 0; i < dynamicMatrix.length; i++) {
                for (int j = 0; j < dynamicMatrix[i].length; j++) {

//                    if (i == codeMatrixIdx[0] && j == codeMatrixIdx[1]) { continue; }

                    DynamicNode node = dynamicMatrix[i][j];
                    if (node.getVehicle().equals(agvNo)) {
                        if (node.getSerial() < serial) {
                            resetCodeList.add(codeMatrix[i][j]);
                        }
                    }
                }
            }

            if (!Cools.isEmpty(resetCodeList)) {

                mapDataDispatcher.clearDynamicMatrixByCodeList(lev, resetCodeList);
            }

            stopWatch.stop();
            if (stopWatch.getTime() > 50) {
                log.info("解锁路径函数花费时间为：{}毫秒......", stopWatch.getTime());
            }

        } catch (Exception e) {
            log.error("MapService.unlockPath", e);
            unlockTaskQueue.offer(new UnlockPathTask(agvNo, codeData), 5, TimeUnit.SECONDS);
        } catch (InterruptedException e) {
            log.error("unlockTaskQueue", e);
        }

    }

    public List<String> getWaveScopeByCodeList(Integer lev, List<String> codeList, Double radiusLen) {
@@ -414,4 +365,103 @@
        return turnMatrixNode == TurnNodeType.TURN.val;
    }

    /**
     * That vehicle is walking if the dynamic node count > 1
     */
    @SuppressWarnings("all")
    public boolean isWalkingByVehicle(Integer lev, String vehicle) {
        if (Cools.isEmpty(vehicle)) {
            return false;
        }

        lev = Optional.ofNullable(lev).orElse(MapDataDispatcher.MAP_DEFAULT_LEV);
        DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);

        int count = 0;
        for (int i = 0; i < dynamicMatrix.length; i++) {
            for (int j = 0; j < dynamicMatrix[i].length; j++) {
                if (vehicle.equals(dynamicMatrix[i][j].getVehicle())) {
                    count++;
                    if (count > 1) {
                        return true;
                    }
                }
            }
        }

        return false;
    }

    public List<String> queryCodeListFromDynamicNode(Integer lev, String nodeType) {
        if (Cools.isEmpty(nodeType)) {
            return new ArrayList<>();
        }
        lev = Optional.ofNullable(lev).orElse(MapDataDispatcher.MAP_DEFAULT_LEV);

        DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
        String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);

        // concurrent
        ConcurrentLinkedQueue<SortCodeDto> codeList = new ConcurrentLinkedQueue<>();
        IntStream.range(0, codeMatrix.length).parallel().forEach(i -> {
            for (int j = 0; j < codeMatrix[i].length; j++) {
                DynamicNode dynamicNode = dynamicMatrix[i][j];
                if (nodeType.equals(dynamicNode.getVehicle())) {
                    codeList.add(new SortCodeDto(codeMatrix[i][j], dynamicNode.getSerial()));
                }
            }
        });

        // synchronize
//        List<SortCodeDto> codeList = new ArrayList<>();
//        for (int i = 0; i < codeMatrix.length; i++) {
//            for (int j = 0; j < codeMatrix[i].length; j++) {
//                DynamicNode dynamicNode = dynamicMatrix[i][j];
//                if (nodeType.equals(dynamicNode.getVehicle())) {
//                    codeList.add(new SortCodeDto(codeMatrix[i][j], dynamicNode.getSerial()));
//                }
//            }
//        }

        return codeList.stream()
                .sorted(Comparator.comparingInt(SortCodeDto::getSerial))
                .map(SortCodeDto::getCode)
                .collect(Collectors.toList());
    }

    public Map<String, List<String>> queryCodeListFromDynamicNode(Integer lev, Set<String> vehicles) {
        if (Cools.isEmpty(vehicles)) {
            return new HashMap<>();
        }
        lev = Optional.ofNullable(lev).orElse(MapDataDispatcher.MAP_DEFAULT_LEV);

        DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
        String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);

        Map<String, List<SortCodeDto>> map = new HashMap<>(vehicles.size());

        for (int i = 0; i < codeMatrix.length; i++) {
            for (int j = 0; j < codeMatrix[i].length; j++) {
                DynamicNode dynamicNode = dynamicMatrix[i][j];
                String vehicle = dynamicNode.getVehicle();

                if (vehicles.contains(vehicle)) {
                    int serial = dynamicNode.getSerial();
                    map.computeIfAbsent(vehicle, k -> new ArrayList<>()).add(new SortCodeDto(codeMatrix[i][j], serial));
                }
            }
        }

        Map<String, List<String>> result = new HashMap<>(vehicles.size());
        for (Map.Entry<String, List<SortCodeDto>> entry : map.entrySet()) {
            List<String> codeDataList = entry.getValue().stream()
                    .sorted(Comparator.comparingInt(SortCodeDto::getSerial))
                    .map(SortCodeDto::getCode)
                    .collect(Collectors.toList());
            result.put(entry.getKey(), codeDataList);
        }

        return result;
    }

}