blame | 历史 | 补丁 | 提交 | 提交对比 | ignore whitespace
zjj
2025-02-17 54b4f53323d0833cbac86fd18932c5fa5a5447a8 
 zy-asrs-wcs/src/main/java/com/zy/asrs/wcs/core/utils/NavigateSolution.java


@@ -1,11 +1,18 @@


package com.zy.asrs.wcs.core.utils;




import com.alibaba.fastjson.JSON;


import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;


import com.zy.asrs.framework.common.SpringUtils;


import com.zy.asrs.framework.exception.CoolException;


import com.zy.asrs.wcs.core.model.NavigateNode;


import com.zy.asrs.wcs.core.model.PythonResult;


import com.zy.asrs.wcs.core.model.enums.MapNodeType;


import com.zy.asrs.wcs.system.entity.Dict;


import com.zy.asrs.wcs.system.service.DictService;




import java.io.BufferedReader;


import java.io.IOException;


import java.io.InputStreamReader;


import java.util.ArrayList;


import java.util.List;


import java.util.PriorityQueue;


@@ -16,7 +23,7 @@


 */


public class NavigateSolution {




    // -1 -> 墙壁， 0 -> 货位， 1 -> 起点  2 -> 终点  3-> 母轨  4->站点


    // -1 -> 墙壁， 0 -> 货位， 1 -> 起点  2 -> 终点  3-> 母轨  4->输送站点  5->充电桩 6->小车可走输送站点  66->小车  67->提升机




    int[][] map = {{}};




@@ -34,7 +41,7 @@


        mapData.setLev(lev);


        int[][] data = mapData.getDataFromRedis(mapType, whitePoints, shuttlePoints);


        if (data == null) {


            data = mapData.getData(mapType, whitePoints, shuttlePoints);


            throw new CoolException("地图未载入！");


        }


        this.map = data;


    }


@@ -50,44 +57,100 @@


    //Exist表用来存放已经出现过的结点。


    public ArrayList<NavigateNode> Exist = new ArrayList<NavigateNode>();




    public NavigateNode astarSearch(NavigateNode start, NavigateNode end) {


        //把第一个开始的结点加入到Open表中


        this.Open.add(start);


        //把出现过的结点加入到Exist表中


        this.Exist.add(start);


        //主循环


        while (Open.size() > 0) {


            //取优先队列顶部元素并且把这个元素从Open表中删除


            NavigateNode current_node = Open.poll();


            //将这个结点加入到Close表中


            Close.add(current_node);


            //对当前结点进行扩展，得到一个四周结点的数组


            ArrayList<NavigateNode> neighbour_node = extend_current_node(current_node);


            //对这个结点遍历，看是否有目标结点出现


            for (NavigateNode node : neighbour_node) {


                // G + H + E (对启发函数增加去拐点方案calcNodeExtraCost)


                int gCost = calcNodeCost(current_node, node) * calcNodeExtraCost(current_node, node, end);


                if (node.getX() == end.getX() && node.getY() == end.getY()) {//找到目标结点就返回


                    //init_node操作把这个邻居结点的父节点设置为当前结点


                    //并且计算出G， F， H等值


                    node.setLastDistance(gCost);


                    node.init_node(current_node, end);


                    return node;


                }


    public String astarSearch(NavigateNode start, NavigateNode end, String pythonScriptPath) {


        String maps = JSON.toJSONString(map);


        String startStr = start.getX() + "-" + start.getY();


        String targetStr = end.getX() + "-" + end.getY();




                //进行计算对G, F, H 等值


                node.setLastDistance(gCost);


                node.init_node(current_node, end);


                node.setH(calcNodeCost(node, end));


                node.setF(node.getG() + node.getH());




                Open.add(node);


                Exist.add(node);


        //默认地图母轨方向x


        String mapDirection = "x";


        DictService dictService = SpringUtils.getBean(DictService.class);


        if (dictService != null) {


            Dict dict = dictService.getOne(new LambdaQueryWrapper<Dict>()


                    .eq(Dict::getFlag, "direction_map")


                    .eq(Dict::getStatus, 1));


            if (dict != null) {


                mapDirection = dict.getValue();


            }


        }


        //如果遍历完所有出现的结点都没有找到最终的结点，返回null




        ProcessBuilder processBuilder = new ProcessBuilder("python", pythonScriptPath, maps, startStr, targetStr, mapDirection);


        processBuilder.redirectErrorStream(true);




        try {


            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;


            }




            PythonResult result = JSON.parseObject(builder.toString(), PythonResult.class);


            if (result.getCalcResult() != 200) {


                return null;


            }




            String path = result.getPath();


            return path;


        } catch (IOException | InterruptedException e) {


            e.printStackTrace();


        }


        return null;


    }




//    public NavigateNode astarSearch(NavigateNode start, NavigateNode end) {


//        //把第一个开始的结点加入到Open表中


//        this.Open.add(start);


//        //把出现过的结点加入到Exist表中


//        this.Exist.add(start);


//        //主循环


//        while (Open.size() > 0) {


//            //取优先队列顶部元素并且把这个元素从Open表中删除


//            NavigateNode current_node = Open.poll();


//            //将这个结点加入到Close表中


//            Close.add(current_node);


//            //对当前结点进行扩展，得到一个四周结点的数组


//            ArrayList<NavigateNode> neighbour_node = extend_current_node(current_node);


//            //对这个结点遍历，看是否有目标结点出现


//            for (NavigateNode node : neighbour_node) {


//                // G + H + E (对启发函数增加去拐点方案calcNodeExtraCost)


//                int gCost = calcNodeCost(current_node, node) * calcNodeExtraCost(current_node, node, end);


//                if (node.getX() == end.getX() && node.getY() == end.getY()) {//找到目标结点就返回


//                    //init_node操作把这个邻居结点的父节点设置为当前结点


//                    //并且计算出G， F， H等值


//                    node.setLastDistance(gCost);


//                    node.init_node(current_node, end);


//                    return node;


//                }


//


//                //进行计算对G, F, H 等值


//                node.setLastDistance(gCost);


//                node.init_node(current_node, end);


//                node.setH(calcNodeCost(node, end));


//                node.setF(node.getG() + node.getH());


//


//                Open.add(node);


//                Exist.add(node);


//            }


//        }


//        //如果遍历完所有出现的结点都没有找到最终的结点，返回null


//        return null;


//    }






    public ArrayList<NavigateNode> extend_current_node(NavigateNode current_node) {


@@ -137,7 +200,7 @@


//        }




        if (mapDirection.equals("x")) {//母轨x方向


            if (map[x][y] == 3) {


            if (map[x][y] == MapNodeType.MAIN_PATH.id) {


                //母轨才能进行上下移动


                if (is_valid(x + 1, y))


                {


@@ -151,8 +214,8 @@


                }


            }




            if (map[x][y] == 0 || map[x][y] == 3 || map[x][y] == 4 || map[x][y] == 5) {


                //子轨和母轨、输送线、充电桩才能进行左右移动


            if (map[x][y] == MapNodeType.NORMAL_PATH.id || map[x][y] == MapNodeType.MAIN_PATH.id || map[x][y] == MapNodeType.CONVEYOR_CAR_GO.id || map[x][y] == MapNodeType.CHARGE.id || map[x][y] == MapNodeType.LIFT.id) {


                //子轨和母轨、小车可走输送线、充电桩、提升机才能进行左右移动


                if (is_valid(x, y + 1))


                {


                    NavigateNode node = new NavigateNode(x, y + 1);


@@ -165,7 +228,7 @@


                }


            }


        }else if (mapDirection.equals("y")) {//母轨y方向


            if (map[x][y] == 3) {


            if (map[x][y] == MapNodeType.MAIN_PATH.id) {


                //母轨才能进行左右移动


                if (is_valid(x, y + 1))


                {


@@ -179,8 +242,8 @@


                }


            }




            if (map[x][y] == 0 || map[x][y] == 3 || map[x][y] == 4 || map[x][y] == 5) {


                //子轨和母轨、输送线、充电桩才能进行上下移动


            if (map[x][y] == MapNodeType.NORMAL_PATH.id || map[x][y] == MapNodeType.MAIN_PATH.id || map[x][y] == MapNodeType.CONVEYOR_CAR_GO.id || map[x][y] == MapNodeType.CHARGE.id || map[x][y] == MapNodeType.LIFT.id) {


                //子轨和母轨、小车可走输送线、充电桩、提升机才能进行上下移动


                if (is_valid(x + 1, y))


                {


                    NavigateNode node = new NavigateNode(x + 1, y);


@@ -203,7 +266,13 @@


            return false;


        }


        // 如果结点的位置小于0，则不合法


        if (map[x][y] < 0) return false;


        if (map[x][y] < 0) {


            return false;


        }




        if (map[x][y] == MapNodeType.CAR.id) {//节点是小车


            return false;


        }


        NavigateNode navigateNode = new NavigateNode(x, y);


        if (is_exist(navigateNode)) {


            return false;