#

Junjie

14 小时以前 7a546480f6ddfaee1366f280981a002a08412c11

 src/main/java/com/zy/common/utils/NavigateUtils.java

@@ -1,257 +1,287 @@
package com.zy.common.utils;

import com.zy.asrs.utils.Utils;
import com.zy.common.model.MapNode;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;

import com.zy.core.News;
import org.springframework.stereotype.Component;

import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.mapper.EntityWrapper;
import com.core.common.SpringUtils;
import com.core.exception.CoolException;
import com.zy.common.model.NavigateNode;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.core.enums.ShuttleTaskModeType;
import com.zy.asrs.entity.DeviceConfig;
import com.zy.asrs.service.DeviceConfigService;
import com.zy.core.cache.SlaveConnection;
import com.zy.core.enums.SlaveType;
import com.zy.core.model.protocol.StationProtocol;
import com.zy.core.thread.StationThread;
import com.zy.system.entity.Config;
import com.zy.system.service.ConfigService;

import java.util.*;

/**
 * A*算法使用工具
 */
@Component
public class NavigateUtils {

    public static List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints) {
        //通过开始编号和结束编号获取对应的xy轴坐标
        int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点
        int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点
    public synchronized List<NavigateNode> calcByStationId(int lev, Integer startStationId, Integer endStationId) {
        NavigateSolution navigateSolution = new NavigateSolution();
        List<List<NavigateNode>> stationMap = navigateSolution.getStationMap(lev);

        ArrayList<int[]> whiteList = new ArrayList<>();//设置计算节点的白名单
        whiteList.add(startArr);//将开始节点设置为白名单，以防被过滤
        NavigateNode startNode = navigateSolution.findStationNavigateNode(stationMap, startStationId);
        if (startNode == null){
            throw new CoolException("未找到该 起点 对应的节点");
        }

        //获取当前节点计算的层高，并赋值到每一个节点中
        int lev = Utils.getLev(startPoint);
        NavigateNode endNode = navigateSolution.findStationNavigateNode(stationMap, endStationId);
        if (endNode == null){
            throw new CoolException("未找到该 终点 对应的节点");
        }

        //初始化开始节点
        NavigateNode start = new NavigateNode(startArr[0], startArr[1]);
        //开始节点无父节点
        start.setFather(null);
        long startTime = System.currentTimeMillis();
        News.info("[WCS Debug] 站点路径开始计算,startStationId={},endStationId={}", startStationId, endStationId);
        List<List<NavigateNode>> allList = navigateSolution.allSimplePaths(stationMap, startNode, endNode, 120, 500, 300);
        if (allList.isEmpty()) {
            throw new CoolException("未找到该路径");
        }
        News.info("[WCS Debug] 站点路径计算完成，耗时：{}ms", System.currentTimeMillis() - startTime);

        NavigateNode end = new NavigateNode(endArr[0], endArr[1]);
        NavigateSolution solution = new NavigateSolution(mapType, lev, whiteList, shuttlePoints);
        //开始节点，不纳入禁用节点内计算
        startTime = System.currentTimeMillis();
        News.info("[WCS Debug] 站点路径权重开始分析,startStationId={},endStationId={}", startStationId, endStationId);
        List<NavigateNode> list = findStationBestPath(allList);
        News.info("[WCS Debug] 站点路径权重分析完成，耗时：{}ms", System.currentTimeMillis() - startTime);

        NavigateNode res_node = solution.astarSearch(start, end);
        //去重
        HashSet<Integer> set = new HashSet<>();
        List<NavigateNode> fitlerList = new ArrayList<>();
        for(NavigateNode navigateNode : list){
            JSONObject valuObject = JSON.parseObject(navigateNode.getNodeValue());
            if(set.add(valuObject.getInteger("stationId"))){
                fitlerList.add(navigateNode);
            }
        }

        return fitlerList;
    }

    public synchronized List<NavigateNode> calcByTrackSiteNo(int lev, Integer startTrackSiteNo, Integer endTrackSiteNo) {
        NavigateSolution navigateSolution = new NavigateSolution();
        List<List<NavigateNode>> rgvTrackMap = navigateSolution.getRgvTrackMap(lev);

        NavigateNode startNode = navigateSolution.findTrackSiteNoNavigateNode(rgvTrackMap, startTrackSiteNo);
        if (startNode == null){
            throw new CoolException("未找到该 起点 对应的节点");
        }

        NavigateNode endNode = navigateSolution.findTrackSiteNoNavigateNode(rgvTrackMap, endTrackSiteNo);
        if (endNode == null){
            throw new CoolException("未找到该 终点 对应的节点");
        }

        long startTime = System.currentTimeMillis();
        News.info("[WCS Debug] RGV路径开始计算,startTrackSiteNo:{},endTrackSiteNo={}", startTrackSiteNo, endTrackSiteNo);
        NavigateNode res_node = navigateSolution.astarSearchJava(rgvTrackMap, startNode, endNode);
        if (res_node == null) {
            System.out.println("未找到路径");
            return null;
        } else {
            ArrayList<NavigateNode> list = new ArrayList<>();
            throw new CoolException("未找到该路径");
        }
        News.info("[WCS Debug] RGV路径计算完成，耗时：{}ms", System.currentTimeMillis() - startTime);

            //渲染
            NavigateNode fatherNode = null;//当前循环上一节点，用于拐点计算
            while (res_node != null) {
                res_node.setDirection(null);
                res_node.setIsInflectionPoint(false);
                res_node.setZ(lev);//设置层高
        ArrayList<NavigateNode> list = new ArrayList<>();
        // 使用 visited 集合防止父链出现环导致死循环，同时设置安全步数上限
        HashSet<NavigateNode> visited = new HashSet<>();
        int maxSteps = rgvTrackMap.size() * rgvTrackMap.get(0).size() + 5; // 安全上限
        int steps = 0;
        while (res_node != null && visited.add(res_node) && steps++ < maxSteps) {
            list.add(res_node);
            res_node = res_node.getFather();//迭代操作
        }
        if (steps >= maxSteps) {
            throw new CoolException("路径回溯超出安全上限，疑似存在父链循环");
        }
        Collections.reverse(list);
        //将每个节点里面的fatherNode至为null(方便后续计算时父节点过多导致显示的节点太多)
        for (NavigateNode navigateNode : list) {
            //父节点设置为null，不影响计算结果，不影响后续操作。
            //此操作仅为后续排查处理提供视觉方便。
            navigateNode.setFather(null);
        }

                //寻找拐点
                HashMap<String, Object> result = searchInflectionPoint(res_node, fatherNode, res_node.getFather());//分别传入当前节点、父节点、下一节点
                //判断当前节点是否为拐点
                if (Boolean.parseBoolean(result.get("result").toString())) {
                    //当前为拐点
                    res_node.setIsInflectionPoint(true);
                    //拐点方向
                    res_node.setDirection(result.get("direction").toString());
        //去重
        HashSet<Integer> set = new HashSet<>();
        List<NavigateNode> fitlerList = new ArrayList<>();
        for(NavigateNode navigateNode : list){
            JSONObject valuObject = JSON.parseObject(navigateNode.getNodeValue());
            if(set.add(valuObject.getInteger("trackSiteNo"))){
                fitlerList.add(navigateNode);
            }
        }

        return fitlerList;
    }

    public synchronized List<NavigateNode> findLiftStationList(int lev) {
        NavigateSolution navigateSolution = new NavigateSolution();
        List<List<NavigateNode>> stationMap = navigateSolution.getStationMap(lev);

        List<NavigateNode> liftStationList = new ArrayList<>();
        for (List<NavigateNode> navigateNodes : stationMap) {
            for (NavigateNode navigateNode : navigateNodes) {
                String nodeType = navigateNode.getNodeType();
                if(nodeType == null){
                    continue;
                }
                list.add(res_node);

                fatherNode = res_node;//把当前节点保存成一个父节点
                res_node = res_node.getFather();//迭代操作
            }

            Collections.reverse(list);

            //将每个节点里面的fatherNode至为null(方便后续计算时父节点过多导致显示的节点太多)
            for (NavigateNode navigateNode : list) {
                //父节点设置为null，不影响计算结果，不影响后续操作。
                //此操作仅为后续排查处理提供视觉方便。
                navigateNode.setFather(null);
            }

            //起始节点计算方向
            String direction = calcDirection(list.get(0), list.get(1));
            NavigateNode startNode = list.get(0);
            startNode.setDirection(direction);
            //更新节点列表
            list.set(0, startNode);
            return list;
        }
    }

    //判断当前节点到下一个节点是否为拐点
    public static HashMap<String,Object> searchInflectionPoint(NavigateNode currentNode, NavigateNode fatherNode, NavigateNode nextNode) {
        HashMap<String, Object> map = new HashMap<>();
        map.put("result", false);//是否为拐点，true：拐点，false：直线
        // 第一个点或直线点
        if (fatherNode == null || nextNode == null || nextNode.getX() == fatherNode.getX() || nextNode.getY() == fatherNode.getY()) {
            return map;//不是拐点直接返回
        }

        //拐点方向
        String direction = calcDirection(currentNode, fatherNode);

        map.put("result", true);//拐点
        map.put("direction", direction);//拐点方向（从当前节点视角看的方向）
        return map;
    }

    /**
     * 计算方向
     */
    public static String calcDirection(NavigateNode currentNode, NavigateNode fatherNode) {
        //拐点方向
        String direction = "";
        // 普通拐点
        //计算拐点方向
        if (fatherNode.getX() != currentNode.getX()) {
            //x轴数据有差异，判断x轴方向
            //当前节点X - 父节点X
            if (currentNode.getX() - fatherNode.getX() > 0) {
                //大于0，方向top
                direction = "top";
            }else {
                //小于0，方向bottom
                direction = "bottom";
            }
        }

        if (fatherNode.getY() != currentNode.getY()) {
            //y轴数据有差异，判断y轴方向
            //当前节点Y - 父节点Y
            if (currentNode.getY() - fatherNode.getY() > 0) {
                //大于0，方向left
                direction = "left";
            }else {
                //小于0，方向right
                direction = "right";
            }
        }

        return direction;
    }

    /**
     * 获取分段路径，每当有一个拐点则进行一次分段，最终返回总分段数据
     */
    public static ArrayList<ArrayList<NavigateNode>> getSectionPath(List<NavigateNode> mapList) {
        ArrayList<ArrayList<NavigateNode>> list = new ArrayList<>();
        ArrayList<NavigateNode> data = new ArrayList<>();
        String direction = mapList.get(0).getDirection();//行走方向

        for (int i = 0; i < mapList.size(); i++) {
            NavigateNode mapNode = mapList.get(i);
            boolean isInflectionPoint = mapNode.getIsInflectionPoint();
            data.add(mapNode);
            if (isInflectionPoint) {
                //拐点
                //分割数据
                list.add(data);//添加某一段数据
                direction = mapNode.getDirection();//更新行走方向
                data = new ArrayList<>();
                data.add(mapNode);//将拐点的终点，更新成下一段命令的起点坐标
            }else {
                //直行线路
                mapNode.setDirection(direction);//设置行走方向
            }
            Integer distance = getXToNextDistance(mapNode);//获取当前点到下一点的行走距离
            mapNode.setMoveDistance(distance);
        }

        //将最后一段数据添加进入
        list.add(data);

        return list;
    }

    //获取从x点到下一点的行走距离
    public static Integer getXToNextDistance(NavigateNode xNode) {
        NavigateMapData mapData = new NavigateMapData();
        List<List<MapNode>> lists = mapData.getJsonData(NavigationMapType.NONE.id, null, null);
        if (lists != null) {
            MapNode mapNode = lists.get(xNode.getX()).get(xNode.getY());
            if (mapNode != null) {
                switch (xNode.getDirection()) {
                    case "top":
                        return mapNode.getTop();
                    case "bottom":
                        return mapNode.getBottom();
                    case "left":
                        return mapNode.getLeft();
                    case "right":
                        return mapNode.getRight();
                if(!nodeType.equals("devp")){
                    continue;
                }
                JSONObject valuObject = JSON.parseObject(navigateNode.getNodeValue());
                if(valuObject == null){
                    continue;
                }
                if (valuObject.containsKey("liftNo")) {
                    liftStationList.add(navigateNode);
                }
            }
            return 0;
        }
        return 0;

        return liftStationList;
    }

    /**
     * 根据原始节点结果，计算总行走距离
     */
    public static Integer getOriginPathAllDistance(List<NavigateNode> path) {
        ArrayList<ArrayList<NavigateNode>> sectionPath = NavigateUtils.getSectionPath(path);
        Integer allDistance = 0;
        for (ArrayList<NavigateNode> navigateNodes : sectionPath) {
            Integer distance = NavigateUtils.getCurrentPathAllDistance(navigateNodes);
            allDistance += distance;
        }
        return allDistance;
    }

    /**
     * 获取当前路径总行走距离
     */
    public static Integer getCurrentPathAllDistance(List<NavigateNode> path) {
        if (path.size() == 1) {
            //路径只有一条数据，则直接返回行走距离
            return path.get(0).getMoveDistance();
    public synchronized List<NavigateNode> findStationBestPath(List<List<NavigateNode>> allList) {
        if (allList == null || allList.isEmpty()) {
            return new ArrayList<>();
        }

        //总距离
        int allDistance = 0;
        for (int i = 0; i < path.size() - 1; i++) {//路径中最后一个节点不统计到总距离，最后一个节点并不会再行走
            allDistance += path.get(i).getMoveDistance();
        }
        return allDistance;
    }

    /**
     * 获取中间点到目标点行走距离
     */
    public static Integer getMiddleToDistDistance(List<NavigateNode> path, NavigateNode middlePath) {
        //总距离
        int allDistance = 0;
        boolean flag = false;
        for (NavigateNode navigateNode : path) {
            if (!flag && navigateNode.equals(middlePath)) {
                flag = true;
        Map<Integer, StationProtocol> statusMap = new HashMap<>();
        try {
            DeviceConfigService deviceConfigService = SpringUtils.getBean(DeviceConfigService.class);
            if (deviceConfigService != null) {
                List<DeviceConfig> devpList = deviceConfigService.selectList(new EntityWrapper<DeviceConfig>()
                        .eq("device_type", String.valueOf(SlaveType.Devp)));
                for (DeviceConfig deviceConfig : devpList) {
                    StationThread stationThread = (StationThread) SlaveConnection.get(SlaveType.Devp, deviceConfig.getDeviceNo());
                    if (stationThread == null) {
                        continue;
                    }
                    Map<Integer, StationProtocol> m = stationThread.getStatusMap();
                    if (m != null && !m.isEmpty()) {
                        statusMap.putAll(m);
                    }
                }
            }
        } catch (Exception ignore) {}

            if (flag) {
                allDistance += navigateNode.getMoveDistance();
        List<List<NavigateNode>> candidates = new ArrayList<>();
        List<Integer> lens = new ArrayList<>();
        List<Integer> tasksList = new ArrayList<>();
        List<Double> congs = new ArrayList<>();

        for (List<NavigateNode> path : allList) {
            if (path == null || path.isEmpty()) {
                continue;
            }
            int len = path.size();
            int tasks = 0;
            HashSet<Integer> stationIdSet = new HashSet<>();
            for (NavigateNode node : path) {
                JSONObject value = null;
                try {
                    value = JSON.parseObject(node.getNodeValue());
                } catch (Exception ignore) {}
                if (value == null) {
                    continue;
                }
                Integer stationId = value.getInteger("stationId");
                if (stationId == null) {
                    continue;
                }
                if (!stationIdSet.add(stationId)) {
                    continue;
                }
                StationProtocol protocol = statusMap.get(stationId);
                if (protocol != null && protocol.getTaskNo() != null && protocol.getTaskNo() > 0) {
                    tasks++;
                }
            }
            double cong = len <= 0 ? 0.0 : (double) tasks / (double) len;
            candidates.add(path);
            lens.add(len);
            tasksList.add(tasks);
            congs.add(cong);
        }

        if (candidates.isEmpty()) {
            return allList.get(0);
        }

        int minLen = Integer.MAX_VALUE;
        int maxLen = Integer.MIN_VALUE;
        double minCong = Double.MAX_VALUE;
        double maxCong = -Double.MAX_VALUE;
        for (int i = 0; i < candidates.size(); i++) {
            int l = lens.get(i);
            double c = congs.get(i);
            if (l < minLen) minLen = l;
            if (l > maxLen) maxLen = l;
            if (c < minCong) minCong = c;
            if (c > maxCong) maxCong = c;
        }

        //长度权重百分比
        double lenWeightPercent = 50.0;
        //拥堵权重百分比
        double congWeightPercent = 50.0;
        try {
            ConfigService configService = SpringUtils.getBean(ConfigService.class);
            if (configService != null) {
                Config cfgLen = configService.selectOne(new EntityWrapper<Config>().eq("code", "stationPathLenWeightPercent"));
                if (cfgLen != null && cfgLen.getValue() != null) {
                    String v = cfgLen.getValue().trim();
                    if (v.endsWith("%")) v = v.substring(0, v.length() - 1);
                    try { lenWeightPercent = Double.parseDouble(v); } catch (Exception ignore) {}
                }
                Config cfgCong = configService.selectOne(new EntityWrapper<Config>().eq("code", "stationPathCongWeightPercent"));
                if (cfgCong != null && cfgCong.getValue() != null) {
                    String v = cfgCong.getValue().trim();
                    if (v.endsWith("%")) v = v.substring(0, v.length() - 1);
                    try { congWeightPercent = Double.parseDouble(v); } catch (Exception ignore) {}
                }
            }
        } catch (Exception ignore) {}

        double weightSum = lenWeightPercent + congWeightPercent;
        double lenW = weightSum <= 0 ? 0.5 : lenWeightPercent / weightSum;
        double congW = weightSum <= 0 ? 0.5 : congWeightPercent / weightSum;

        List<NavigateNode> best = null;
        double bestCost = Double.MAX_VALUE;
        int bestTasks = Integer.MAX_VALUE;
        int bestLen = Integer.MAX_VALUE;
        for (int i = 0; i < candidates.size(); i++) {
            int l = lens.get(i);
            int t = tasksList.get(i);
            double c = congs.get(i);
            //归一化
            double lenNorm = (maxLen - minLen) <= 0 ? 0.0 : (l - minLen) / (double) (maxLen - minLen);
            double congNorm = (maxCong - minCong) <= 0 ? 0.0 : (c - minCong) / (double) (maxCong - minCong);
            //获取权重
            double cost = lenNorm * lenW + congNorm * congW;
            if (cost < bestCost
                    || (cost == bestCost && t < bestTasks)
                    || (cost == bestCost && t == bestTasks && l < bestLen)) {
                best = candidates.get(i);
                bestCost = cost;
                bestTasks = t;
                bestLen = l;
            }
        }
        return allDistance;
    }

    public static void main(String[] args) {
        //计算路径
        List<NavigateNode> calc = calc("1000901", "1800201", NavigationMapType.NONE.id, null);
        System.out.println(calc);
        System.out.println("------------------------");
//        List<NavigateNode> calc = calc("0501401", "0201801", "out");
        //将路径分割成多段
        ArrayList<ArrayList<NavigateNode>> data = getSectionPath(calc);
        for (ArrayList<NavigateNode> list : data) {
            Integer currentPathAllDistance = getCurrentPathAllDistance(list);//计算当前路径总距离
            System.out.println(currentPathAllDistance);
            System.out.println(list);
        if (best == null) {
            return allList.get(0);
        }

        return best;
    }

}