#

Junjie

6 天以前 e2cbfe96c6dea03c15bbb8ea8c7af320242a5d19

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

@@ -1,88 +1,194 @@
package com.zy.common.utils;

import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONArray;
import com.core.common.SpringUtils;
import com.zy.asrs.utils.Utils;
import com.zy.common.model.MapNode;
import com.zy.common.model.NavigateNode;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.core.enums.ShuttleTaskModeType;
import com.zy.core.News;
import com.zy.core.enums.MapNodeType;
import com.zy.core.model.PythonSimilarityResult;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.stereotype.Component;

import java.io.BufferedReader;
import java.io.InputStreamReader;
import java.util.*;

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

    public static List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints) {
    @Value("${pythonCalcPath}")
    private String pythonCalcPath;
    @Value("${pythonCalcSimilarity}")
    private String pythonCalcSimilarity;
    @Autowired
    private NavigateMapData navigateMapData;

    public List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints, List<int[]> whites) {
        return calcJava(startPoint, endPoint, mapType, shuttlePoints, whites);
    }

    public List<NavigateNode> calcJava(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints, List<int[]> whites) {
        //通过开始编号和结束编号获取对应的xy轴坐标
        int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点
        int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点

        ArrayList<int[]> whiteList = new ArrayList<>();//设置计算节点的白名单
        whiteList.add(startArr);//将开始节点设置为白名单，以防被过滤
        if (whites != null && !whites.isEmpty()) {
            whiteList.addAll(whites);//批量添加白名单节点
        }

        //获取当前节点计算的层高，并赋值到每一个节点中
        int lev = Utils.getLev(startPoint);
        NavigateSolution solution = new NavigateSolution(mapType, lev, whiteList, shuttlePoints);
        int[][] map = solution.map;

        //初始化开始节点
        NavigateNode start = new NavigateNode(startArr[0], startArr[1]);
        //开始节点无父节点
        start.setFather(null);
        start.setNodeValue(map[startArr[0]][startArr[1]]);

        NavigateNode end = new NavigateNode(endArr[0], endArr[1]);
        NavigateSolution solution = new NavigateSolution(mapType, lev, whiteList, shuttlePoints);
        end.setNodeValue(map[endArr[0]][endArr[1]]);
        //开始节点，不纳入禁用节点内计算

        NavigateNode res_node = solution.astarSearch(start, end);
        NavigateNode res_node = solution.astarSearchJava(start, end);
        if (res_node == null) {
            System.out.println("未找到路径");
            News.error("{} dash {} can't find navigate path!", startPoint, endPoint);
            return null;
        } else {
            ArrayList<NavigateNode> list = new ArrayList<>();

            //渲染
            NavigateNode fatherNode = null;//当前循环上一节点，用于拐点计算
            while (res_node != null) {
                res_node.setDirection(null);
                res_node.setIsInflectionPoint(false);
                res_node.setZ(lev);//设置层高

                //寻找拐点
                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());
                }
                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;
        }

        ArrayList<NavigateNode> list = new ArrayList<>();
        //渲染
        NavigateNode fatherNode = null;//当前循环上一节点，用于拐点计算
        while (res_node != null) {
            res_node.setDirection(null);
            res_node.setIsInflectionPoint(false);
            res_node.setZ(lev);//设置层高

            //寻找拐点
            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());
            }
            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 List<NavigateNode> calcPython(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints, List<int[]> whites) {
        //通过开始编号和结束编号获取对应的xy轴坐标
        int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点
        int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点

        ArrayList<int[]> whiteList = new ArrayList<>();//设置计算节点的白名单
        whiteList.add(startArr);//将开始节点设置为白名单，以防被过滤
        if (whites != null && !whites.isEmpty()) {
            whiteList.addAll(whites);//批量添加白名单节点
        }

        //获取当前节点计算的层高，并赋值到每一个节点中
        int lev = Utils.getLev(startPoint);
        NavigateSolution solution = new NavigateSolution(mapType, lev, whiteList, shuttlePoints);
        int[][] map = solution.map;

        //初始化开始节点
        NavigateNode start = new NavigateNode(startArr[0], startArr[1]);
        start.setNodeValue(map[startArr[0]][startArr[1]]);
        //开始节点无父节点
        start.setFather(null);

        NavigateNode end = new NavigateNode(endArr[0], endArr[1]);
        end.setNodeValue(map[endArr[0]][endArr[1]]);
        //开始节点，不纳入禁用节点内计算

        String pathStr = solution.astarSearchPython(start, end, pythonCalcPath);
        if (pathStr == null) {
            News.error("{} dash {} can't find navigate path!", startPoint, endPoint);
            return null;
        }

        List<NavigateNode> list = new ArrayList<>();

        NavigateNode fatherNode = null;//当前循环上一节点，用于拐点计算
        JSONArray rows = JSON.parseArray(pathStr);

        for (int i = 0; i < rows.size(); i++) {
            Object currentObj = rows.get(i);

            NavigateNode nextNode = null;
            if ((i + 1) < rows.size()) {
                Object object = rows.get(i + 1);
                nextNode = pythonObjTransferNode(object, lev);
            }

            NavigateNode node = pythonObjTransferNode(currentObj, lev);

            //寻找拐点
            HashMap<String, Object> result = searchInflectionPoint(node, fatherNode, nextNode);//分别传入当前节点、父节点、下一节点
            //判断当前节点是否为拐点
            if (Boolean.parseBoolean(result.get("result").toString())) {
                //当前为拐点
                node.setIsInflectionPoint(true);
                //拐点方向
                node.setDirection(result.get("direction").toString());
            }

            list.add(node);

            fatherNode = node;//把当前节点保存成一个父节点
        }

        //将每个节点里面的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) {
    public HashMap<String,Object> searchInflectionPoint(NavigateNode currentNode, NavigateNode fatherNode, NavigateNode nextNode) {
        HashMap<String, Object> map = new HashMap<>();
        map.put("result", false);//是否为拐点，true：拐点，false：直线
        // 第一个点或直线点
@@ -101,7 +207,7 @@
    /**
     * 计算方向
     */
    public static String calcDirection(NavigateNode currentNode, NavigateNode fatherNode) {
    public String calcDirection(NavigateNode currentNode, NavigateNode fatherNode) {
        //拐点方向
        String direction = "";
        // 普通拐点
@@ -134,30 +240,30 @@
    }

    /**
     * 加转弯节点
     * 获取分段路径，每当有一个拐点则进行一次分段，最终返回总分段数据
     */
    public static ArrayList<ArrayList<NavigateNode>> getSectionPath(List<NavigateNode> mapList) {
    public 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) {
                //拐点
        for (NavigateNode navigateNode : mapList) {
            data.add(navigateNode);
            //拐点
            if (navigateNode.getIsInflectionPoint()) {
                //分割数据
                list.add(data);//添加某一段数据
                direction = mapNode.getDirection();//更新行走方向
                direction = navigateNode.getDirection();//更新行走方向
                data = new ArrayList<>();
                data.add(mapNode);//将拐点的终点，更新成下一段命令的起点坐标
            }else {
                data.add(navigateNode);//将拐点的终点，更新成下一段命令的起点坐标
            } else {
                //直行线路
                mapNode.setDirection(direction);//设置行走方向
                navigateNode.setDirection(direction);//设置行走方向
            }
            Integer distance = getXToNextDistance(mapNode);//获取当前点到下一点的行走距离
            mapNode.setMoveDistance(distance);
            Integer distance = getXToNextDistance(navigateNode);//获取当前点到下一点的行走距离
            navigateNode.setMoveDistance(distance);
        }

        //将最后一段数据添加进入
@@ -167,9 +273,9 @@
    }

    //获取从x点到下一点的行走距离
    public static Integer getXToNextDistance(NavigateNode xNode) {
        NavigateMapData mapData = new NavigateMapData();
        List<List<MapNode>> lists = mapData.getJsonData(NavigationMapType.NONE.id, null, null);
    public Integer getXToNextDistance(NavigateNode xNode) {
        NavigateMapData mapData = SpringUtils.getBean(NavigateMapData.class);
        List<List<MapNode>> lists = mapData.getJsonData(xNode.getZ(), NavigationMapType.NONE.id, null, null);
        if (lists != null) {
            MapNode mapNode = lists.get(xNode.getX()).get(xNode.getY());
            if (mapNode != null) {
@@ -192,11 +298,11 @@
    /**
     * 根据原始节点结果，计算总行走距离
     */
    public static Integer getOriginPathAllDistance(List<NavigateNode> path) {
        ArrayList<ArrayList<NavigateNode>> sectionPath = NavigateUtils.getSectionPath(path);
    public Integer getOriginPathAllDistance(List<NavigateNode> path) {
        ArrayList<ArrayList<NavigateNode>> sectionPath = getSectionPath(path);
        Integer allDistance = 0;
        for (ArrayList<NavigateNode> navigateNodes : sectionPath) {
            Integer distance = NavigateUtils.getCurrentPathAllDistance(navigateNodes);
            Integer distance = getCurrentPathAllDistance(navigateNodes);
            allDistance += distance;
        }
        return allDistance;
@@ -205,7 +311,7 @@
    /**
     * 获取当前路径总行走距离
     */
    public static Integer getCurrentPathAllDistance(List<NavigateNode> path) {
    public Integer getCurrentPathAllDistance(List<NavigateNode> path) {
        if (path.size() == 1) {
            //路径只有一条数据，则直接返回行走距离
            return path.get(0).getMoveDistance();
@@ -222,25 +328,119 @@
    /**
     * 获取中间点到目标点行走距离
     */
    public static Integer getMiddleToDistDistance(List<NavigateNode> path) {
        //中间路径
        NavigateNode middlePath = path.get(path.size() - 2);
        return middlePath.getMoveDistance();
    public Integer getMiddleToDistDistance(List<NavigateNode> path, NavigateNode middlePath) {
        //最后一条节点不计算进行走距离
        NavigateNode lastPath = path.get(path.size() - 1);
        //总距离
        int allDistance = 0;
        boolean flag = false;
        for (NavigateNode navigateNode : path) {
            if (!flag && navigateNode.equals(middlePath)) {
                flag = true;
            }

            if (navigateNode.equals(lastPath)) {
                continue;//最后一条节点不计算进行走距离
            }

            if (flag) {
                allDistance += navigateNode.getMoveDistance();
            }
        }
        return allDistance;
    }

    /**
     * 检测路径是否可用(可走)
     */
    public boolean checkPathIsAvailable(List<NavigateNode> path, Integer shuttleNo, Integer lev) {
        int[][] map = navigateMapData.getDataFromRedis(lev, NavigationMapType.DFX.id, null, Utils.getShuttlePoints(shuttleNo, lev));
        for (NavigateNode node : path) {
            int value = map[node.getX()][node.getY()];
            if (value != MapNodeType.NORMAL_PATH.id && value != MapNodeType.MAIN_PATH.id && value != MapNodeType.CHARGE.id && value != MapNodeType.CONVEYOR_CAR_GO.id) {//母轨道3、子轨道0、充电桩5、小车可走输送站
                return false;
            }
        }
        return true;
    }

    private NavigateNode pythonObjTransferNode(Object obj, Integer z) {
        List<Integer> pathData = JSON.parseArray(obj.toString(), Integer.class);
        Integer x = pathData.get(0);
        Integer y = pathData.get(1);

        NavigateNode node = new NavigateNode(x, y);
        node.setDirection(null);
        node.setIsInflectionPoint(false);
        node.setZ(z);
        return node;
    }

    public Double similarityPath(List<NavigateNode> firstPath, List<NavigateNode> secondPath) {
        try {
            List<int[]> first = new ArrayList<>();
            for (NavigateNode node : firstPath) {
                first.add(new int[]{node.getX(), node.getY()});
            }

            List<int[]> second = new ArrayList<>();
            for (NavigateNode node : secondPath) {
                second.add(new int[]{node.getX(), node.getY()});
            }

            ProcessBuilder processBuilder = new ProcessBuilder("python", pythonCalcSimilarity, JSON.toJSONString(first), JSON.toJSONString(second));
            processBuilder.redirectErrorStream(true);

            Process process = processBuilder.start();

            // 读取Python脚本的输出
            BufferedReader reader = new BufferedReader(new InputStreamReader(process.getInputStream()));
            String line;
            StringBuilder builder = new StringBuilder();
            while ((line = reader.readLine()) != null) {
                builder.append(line);
            }

            // 等待Python脚本执行完成
            int exitCode = process.waitFor();
            if (exitCode != 0) {
                System.out.println("Python script exited with error code: " + exitCode);
                return null;
            }
            reader.close();

            if (builder.length() <= 0) {
                return null;
            }

            PythonSimilarityResult result = JSON.parseObject(builder.toString(), PythonSimilarityResult.class);
            if (result.getCalcResult() != 200) {
                return 0D;
            }

            Double similarity = result.getSimilarity();
            return similarity;
        } catch (Exception e) {
            e.printStackTrace();
        }
        return 0D;
    }

    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);
        }
//        //计算路径
//        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);
//        }

        System.loadLibrary("example");

    }