寻路算法，地图节点优化，剔除原入出库模式获取地图节点方案，改为根据库位状态实时获取节点信息或直接获取无过滤的完整节点信息

Junjie

2023-06-18 9f05696822f8feee385a34a92dd9aa16009e84a0

寻路算法，地图节点优化，剔除原入出库模式获取地图节点方案，改为根据库位状态实时获取节点信息或直接获取无过滤的完整节点信息

 src/main/java/com/zy/asrs/controller/ConsoleController.java

@@ -18,6 +18,7 @@
import com.zy.asrs.service.WrkMastService;
import com.zy.common.CodeRes;
import com.zy.common.model.MapNode;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.common.utils.NavigateMapData;
import com.zy.common.utils.RedisUtil;
import com.zy.core.CrnThread;
@@ -316,7 +317,7 @@
        //解析json地图数据
        ArrayList arrayList = JSON.parseObject(basMap.getData(), ArrayList.class);
        NavigateMapData navigateMapData = new NavigateMapData(lev);
        List<List<MapNode>> lists = navigateMapData.filterMap(-1, arrayList);//过滤地图数据
        List<List<MapNode>> lists = navigateMapData.filterMap(NavigationMapType.NONE.id, arrayList, lev, null);//过滤地图数据
        return R.ok().add(lists);
    }


 src/main/java/com/zy/asrs/mapper/LocMastMapper.java

@@ -37,4 +37,6 @@
    LocMast queryByLoc(String locNo);

    LocMast queryByQrCode(String qrCodeValue);

    List<LocMast> selectLocByLev(Integer lev);
}

 src/main/java/com/zy/asrs/service/LocMastService.java

@@ -67,4 +67,7 @@
    LocMast queryByLoc(String locNo);

    LocMast queryByQrCode(String qrCodeValue);

    //查询指定楼层的库位数据
    List<LocMast> selectLocByLev(Integer lev);
}

 src/main/java/com/zy/asrs/service/impl/LocMastServiceImpl.java

@@ -86,4 +86,9 @@
    public LocMast queryByQrCode(String qrCodeValue) {
        return this.baseMapper.queryByQrCode(qrCodeValue);
    }

    @Override
    public List<LocMast> selectLocByLev(Integer lev) {
        return this.baseMapper.selectLocByLev(lev);
    }
}

 src/main/java/com/zy/asrs/service/impl/MainServiceImpl.java

@@ -10,6 +10,7 @@
import com.zy.asrs.service.*;
import com.zy.asrs.utils.Utils;
import com.zy.common.model.*;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.common.model.enums.WrkChargeType;
import com.zy.common.service.CommonService;
import com.zy.common.service.erp.ErpService;
@@ -569,7 +570,7 @@
                    //数据库中也不存在地图数据，从地图文件中获取
                    //载入地图
                    NavigateMapData mapData = new NavigateMapData(i);
                    List<List<MapNode>> lists = mapData.getJsonData(-1);//获取完整地图(包括入库出库)
                    List<List<MapNode>> lists = mapData.getJsonData(-1, null);//获取完整地图(包括入库出库)

                    //存入数据库
                    basMap = new BasMap();
@@ -796,7 +797,7 @@

    }

    //获取起点-终点指令。mapType：0=>无货物地图，1=》携带货物出入库地图
    //获取起点-终点指令
    public synchronized List<ShuttleCommand> shuttleAssignCommand(String startLocNo, String locNo, Integer mapType, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
        //获取小车移动速度
        BasShuttle basShuttle = basShuttleService.selectById(assignCommand.getShuttleNo());
@@ -864,7 +865,7 @@
        List<NavigateNode> allNode = new ArrayList<>();

        //计算小车起点到中点所需命令
        List<NavigateNode> calc = NavigateUtils.calc(startLocNo, middleLocNo, 1);//小车无货，走入库地图
        List<NavigateNode> calc = NavigateUtils.calc(startLocNo, middleLocNo, NavigationMapType.NORMAL.id);//小车无货，走正常库位通道
        List<ShuttleCommand> commands = new ArrayList<>();

        if (calc != null) {
@@ -898,7 +899,7 @@
        commands.add(shuttleThread.getPalletCommand((short) 1));

        //计算小车中点到终点所需命令
        List<NavigateNode> calc2 = NavigateUtils.calc(middleLocNo, locNo, 2);//小车有货，走出库地图(出库地图有专用货道)
        List<NavigateNode> calc2 = NavigateUtils.calc(middleLocNo, locNo, NavigationMapType.DFX.id);//小车有货，走禁用过DFX库位的地图通道
        if (calc2 == null) {
            return null;
        }
@@ -1209,9 +1210,9 @@
                String recentLocNo = recentShuttle.getShuttleProtocol().getCurrentLocNo();

                //当前最近四向穿梭车到目标地点距离
                List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, distLocNo, ShuttleTaskModeType.PAK_IN.id);
                List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, distLocNo, NavigationMapType.NORMAL.id);//搜索空闲穿梭车，使用正常通道地图
                //当前穿梭车线程到目标地点距离
                List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, distLocNo, ShuttleTaskModeType.PAK_IN.id);
                List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, distLocNo, NavigationMapType.NORMAL.id);//搜索空闲穿梭车，使用正常通道地图
                if (recentShuttlePath == null || currentShuttlePath == null) {
                    continue;
                }
@@ -1241,9 +1242,9 @@
                int recentLev = Utils.getLev(recentLocNo);

                //当前最近四向穿梭车到当前车子所在楼层的提升机口距离
                List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, Utils.levToOutInStaLocNo(recentLev), ShuttleTaskModeType.PAK_IN.id);
                List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, Utils.levToOutInStaLocNo(recentLev), NavigationMapType.NORMAL.id);//搜索空闲穿梭车，使用正常通道地图
                //当前穿梭车线程到当前车子所在楼层的提升机口距离
                List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, Utils.levToOutInStaLocNo(currentLev), ShuttleTaskModeType.PAK_IN.id);
                List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, Utils.levToOutInStaLocNo(currentLev), NavigationMapType.NORMAL.id);//搜索空闲穿梭车，使用正常通道地图
                if (recentShuttlePath == null || currentShuttlePath == null) {
                    continue;
                }

 src/main/java/com/zy/common/model/enums/NavigationMapType.java

New file
@@ -0,0 +1,42 @@
package com.zy.common.model.enums;

public enum NavigationMapType {

    NONE(-1, "无过滤"),
    DFX(1, "过滤库位状态DFX"),
    NORMAL(2, "过滤库位状态X"),
    ;

    public Integer id;
    public String desc;

    NavigationMapType(Integer id, String desc) {
        this.id = id;
        this.desc = desc;
    }

    public static NavigationMapType get(Short id) {
        if (null == id) {
            return null;
        }
        for (NavigationMapType type : NavigationMapType.values()) {
            if (type.id.equals(id.intValue())) {
                return type;
            }
        }
        return null;
    }

    public static NavigationMapType get(NavigationMapType type) {
        if (null == type) {
            return null;
        }
        for (NavigationMapType type1 : NavigationMapType.values()) {
            if (type1 == type) {
                return type1;
            }
        }
        return null;
    }

}

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

@@ -1,10 +1,14 @@
package com.zy.common.utils;

import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONObject;
import com.core.common.SpringUtils;
import com.zy.asrs.entity.BasMap;
import com.zy.asrs.entity.LocMast;
import com.zy.asrs.service.LocMastService;
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 org.springframework.stereotype.Component;

@@ -30,10 +34,10 @@
    }

    public int[][] getData() {
        return getData(ShuttleTaskModeType.PAK_IN.id);
        return getData(NavigationMapType.NONE.id, null);//默认读取无过滤的全部地图数据
    }

    public int[][] getData(Integer mapType) {
    public int[][] getData(Integer mapType, List<int[]> whitePoints) {
        try {
            String mapFilename = "map_" + lev + ".json";

@@ -51,7 +55,7 @@

                //解析json地图数据
                ArrayList arrayList = JSON.parseObject(stringBuffer.toString(), ArrayList.class);
                List<List<MapNode>> lists = filterMap(mapType, arrayList);//过滤地图数据
                List<List<MapNode>> lists = filterMap(mapType, arrayList, lev, whitePoints);//过滤地图数据
                int[][] map = new int[lists.size()][];
                int j = 0;
                for (List<MapNode> list : lists) {
@@ -78,7 +82,7 @@
    /**
     * 尝试从redis获取数据
     */
    public int[][] getDataFromRedis(Integer mapType) {
    public int[][] getDataFromRedis(Integer mapType, List<int[]> whitePoints) {
        RedisUtil redisUtil = SpringUtils.getBean(RedisUtil.class);
        Object o = redisUtil.get("realtimeBasMap_" + lev);
        if (o == null) {
@@ -86,15 +90,15 @@
        }

        BasMap basMap = JSON.parseObject(o.toString(), BasMap.class);
        return this.getDataFormString(basMap.getData(), mapType);
        return this.getDataFormString(basMap.getData(), mapType, whitePoints);
    }

    /**
     * 从List数据中获取地图
     */
    public int[][] getDataFormString(String data, Integer mapType) {
    public int[][] getDataFormString(String data, Integer mapType, List<int[]> whitePoints) {
        ArrayList arrayList = JSON.parseObject(data, ArrayList.class);
        List<List<MapNode>> lists = filterMap(mapType, arrayList);//过滤地图数据
        List<List<MapNode>> lists = filterMap(mapType, arrayList, lev, whitePoints);//过滤地图数据
        int[][] map = new int[lists.size()][];
        int j = 0;
        for (List<MapNode> list : lists) {
@@ -112,7 +116,7 @@
    }

    //获取JSON格式数据
    public List<List<MapNode>> getJsonData(Integer mapType) {
    public List<List<MapNode>> getJsonData(Integer mapType, List<int[]> whitePoints) {
        try {
            String mapFilename = "map_" + lev + ".json";

@@ -130,7 +134,7 @@

                //解析json地图数据
                ArrayList arrayList = JSON.parseObject(stringBuffer.toString(), ArrayList.class);
                List<List<MapNode>> lists = filterMap(mapType, arrayList);//过滤地图数据
                List<List<MapNode>> lists = filterMap(mapType, arrayList, lev, whitePoints);//过滤地图数据

                return lists;
            } else {
@@ -143,31 +147,68 @@
    }

    /**
     * 过滤地图数据，入库操作则过滤出库的禁用库位，出库操作则过滤入库的禁用库位
     * 过滤地图数据
     * mapType -1=>无过滤，1=》过滤库位状态DFX，2=》过滤库位状态X
     * @param whitePoints 白名单节点，不需要被过滤
     */
    public List<List<MapNode>> filterMap(Integer mapType,List arrayList) {
    public List<List<MapNode>> filterMap(Integer mapType, List arrayList, Integer lev, List<int[]> whitePoints) {
        List<List<MapNode>> lists = new ArrayList<>();

        //重建数据格式
        for (int i = 0; i < arrayList.size(); i++) {
            Object obj = arrayList.get(i);
            List<MapNode> list = JSON.parseArray(obj.toString(), MapNode.class);
            for (int j = 0; j < list.size(); j++) {
                MapNode mapNode = list.get(j);
                if (mapType == ShuttleTaskModeType.PAK_IN.id) {
                    //入库地图
                    if (mapNode.getData().equals("IN_X")) {
                        //禁止使用的库位，设置为-1
                        mapNode.setValue(-1);
                    }
                }else if(mapType == ShuttleTaskModeType.PAK_OUT.id){
                    //出库地图
                    if (mapNode.getData().equals("OUT_X")) {
                        //禁止使用的库位，设置为-1
                        mapNode.setValue(-1);
                    }
                }
                list.set(j, mapNode);
            }
            lists.add(list);
        }

        //过滤数据
        LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
        //获取当前楼层库位数据
        List<LocMast> locMasts = locMastService.selectLocByLev(lev);
        for (LocMast locMast : locMasts) {
            Integer row = locMast.getRow1();
            Integer bay = locMast.getBay1();

            boolean whiteFlag = false;//默认不存在白名单
            if (whitePoints != null) {
                for (int[] whitePoint : whitePoints) {
                    if (whitePoint[0] == row && whitePoint[1] == bay) {
                        //存在白名单
                        whiteFlag = true;
                        break;
                    }
                }
            }
            if (whiteFlag) {
                continue;//存在白名单，不执行下列过滤方案
            }


            List<MapNode> list = lists.get(row);
            MapNode mapNode = list.get(bay);

            if (mapType == NavigationMapType.NONE.id) {
                //不过滤任何数据
            } else if (mapType == NavigationMapType.DFX.id) {
                //车辆有货
                //读取对应库位数据，将DFX库位状态的节点置为-1(障碍物)
                if (locMast.getLocSts().equals("F") || locMast.getLocSts().equals("D") || locMast.getLocSts().equals("X")) {
                    mapNode.setValue(-1);//禁用节点
                }
            } else if (mapType == NavigationMapType.NORMAL.id) {
                //过滤库位状态X
                if (locMast.getLocSts().equals("X")) {
                    mapNode.setValue(-1);//禁用节点
                }
            }

            //更新list
            list.set(bay, mapNode);
            lists.set(row, list);
        }

        return lists;
@@ -186,7 +227,7 @@

        BasMap basMap = JSON.parseObject(o.toString(), BasMap.class);
        ArrayList arrayList = JSON.parseObject(basMap.getData(), ArrayList.class);
        List<List<MapNode>> lists = filterMap(-1, arrayList);//获取全部地图数据
        List<List<MapNode>> lists = filterMap(NavigationMapType.NONE.id, arrayList, lev, null);//获取全部地图数据

        for (NavigateNode node : nodes) {
            if (node.getZ() != lev) {

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

@@ -3,6 +3,7 @@
import com.zy.common.model.NavigateNode;

import java.util.ArrayList;
import java.util.List;
import java.util.PriorityQueue;

/**
@@ -21,12 +22,12 @@
        this.map = data;
    }

    public NavigateSolution(Integer mapType,Integer lev) {
    public NavigateSolution(Integer mapType, Integer lev, List<int[]> whitePoints) {
        //载入地图指定层高地图
        NavigateMapData mapData = new NavigateMapData(lev);
        int[][] data = mapData.getDataFromRedis(mapType);
        int[][] data = mapData.getDataFromRedis(mapType, whitePoints);
        if (data == null) {
            data = mapData.getData(mapType);
            data = mapData.getData(mapType, whitePoints);
        }
        this.map = data;
    }

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

@@ -3,6 +3,7 @@
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 java.util.*;
@@ -17,6 +18,9 @@
        int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点
        int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点

        ArrayList<int[]> whiteList = new ArrayList<>();//设置计算节点的白名单
        whiteList.add(startArr);//将开始节点设置为白名单，以防被过滤

        //获取当前节点计算的层高，并赋值到每一个节点中
        int lev = Utils.getLev(startPoint);

@@ -26,7 +30,9 @@
        start.setFather(null);

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

        NavigateNode res_node = solution.astarSearch(start, end);
        if (res_node == null) {
            System.out.println("未找到路径");
@@ -163,7 +169,7 @@
    //获取从x点到下一点的行走距离
    public static Integer getXToNextDistance(NavigateNode xNode) {
        NavigateMapData mapData = new NavigateMapData();
        List<List<MapNode>> lists = mapData.getJsonData(1);
        List<List<MapNode>> lists = mapData.getJsonData(NavigationMapType.NONE.id, null);
        if (lists != null) {
            MapNode mapNode = lists.get(xNode.getX()).get(xNode.getY());
            if (mapNode != null) {
@@ -224,7 +230,7 @@

    public static void main(String[] args) {
        //计算路径
        List<NavigateNode> calc = calc("1000901", "1800201", ShuttleTaskModeType.PAK_OUT.id);
        List<NavigateNode> calc = calc("1000901", "1800201", NavigationMapType.NONE.id);
        System.out.println(calc);
        System.out.println("------------------------");
//        List<NavigateNode> calc = calc("0501401", "0201801", "out");

 src/main/java/com/zy/core/thread/ShuttleThread.java

@@ -12,6 +12,7 @@
import com.zy.asrs.service.*;
import com.zy.asrs.utils.Utils;
import com.zy.common.model.NavigateNode;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.common.utils.*;
import com.zy.core.News;
import com.zy.core.ThreadHandler;
@@ -414,7 +415,7 @@
                    //小车移动到提升机口，计算路径
                    //计算小车起点到中点所需命令
                    LocMast currentLocMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
                    List<NavigateNode> firstMastResult = NavigateUtils.calc(currentLocMast.getLocNo(), assignCommand.getSourceLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    List<NavigateNode> firstMastResult = NavigateUtils.calc(currentLocMast.getLocNo(), assignCommand.getSourceLocNo(), NavigationMapType.NORMAL.id);//小车到中点，处于无货状态，使用正常通道地图

                    if (firstMastResult != null) {
                        allNode.addAll(firstMastResult);//将节点进行保存
@@ -450,7 +451,7 @@
                    }

                    //计算中点到终点路径
                    List<NavigateNode> secMastResult = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    List<NavigateNode> secMastResult = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), NavigationMapType.DFX.id);//小车从中点到终点，处于有货状态，使用DFX地图

                    if (secMastResult != null) {
                        allNode.addAll(secMastResult);//将节点进行保存
@@ -478,15 +479,14 @@
                        commands.add(getPalletCommand((short) 2));
                    }

                    navigateMapData = new NavigateMapData(Utils.getLev(currentLocMast.getLocNo()));
                    if (firstMastResult != null) {
                        //所使用的路径进行锁定禁用
                        navigateMapData.writeNavigateNodeToRedisMap(firstMastResult, true);////所使用的路径进行锁定禁用
                    if (firstMastResult == null || secMastResult == null) {
                        throw new CoolException(MessageFormat.format( "四向穿梭车出入库路径搜索失败 ===>> [id:{0}] [ip:{1}] [port:{2}]", slave.getId(), slave.getIp(), slave.getPort()));
                    }

                    if (secMastResult != null) {
                    navigateMapData = new NavigateMapData(Utils.getLev(currentLocMast.getLocNo()));
                    //所使用的路径进行锁定禁用
                    navigateMapData.writeNavigateNodeToRedisMap(firstMastResult, true);////所使用的路径进行锁定禁用
                        navigateMapData.writeNavigateNodeToRedisMap(secMastResult, true);////所使用的路径进行锁定禁用
                    }
                    break;
                case 3://托盘顶升
                case 4://托盘下降
@@ -536,7 +536,7 @@
                    }

                    LocMast locMast = locMastService.queryByQrCode(startQr);
                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), assignCommand.getLocNo(), NavigationMapType.NONE.id);//手动命令-移动命令，使用无过滤地图

                    if (result != null) {
                        //所使用的路径进行锁定禁用
@@ -572,7 +572,7 @@
                    int lev = Utils.getLev(locMast1.getLocNo());//穿梭车当前高度
                    String liftSiteLocNo = Utils.levToOutInStaLocNo(lev);//当前楼层站点库位号
                    LocMast liftSitelocMast = locMastService.selectById(liftSiteLocNo);
                    List<NavigateNode> result1 = NavigateUtils.calc(locMast1.getLocNo(), liftSiteLocNo, ShuttleTaskModeType.PAK_IN.id);
                    List<NavigateNode> result1 = NavigateUtils.calc(locMast1.getLocNo(), liftSiteLocNo, NavigationMapType.NONE.id);//移动到提升机，使用无过滤地图

                    Short endStartCode = null;
                    if (result1 != null) {
@@ -675,7 +675,7 @@
                    shuttleProtocol.setPakMk(true);
                    return true;
                }else {
                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), distLocMast.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), distLocMast.getLocNo(), NavigationMapType.DFX.id);//错误恢复，使用DFX地图
                    if (result != null) {
                        //获取分段路径
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(result);

 src/main/resources/mapper/LocMastMapper.xml

@@ -70,4 +70,7 @@
        select top 1 * from asr_loc_mast where qr_code_value = #{qrCodeValue}
    </select>

    <select id="selectLocByLev" resultMap="BaseResultMap">
        SELECT * FROM asr_loc_mast WHERE lev1 = #{lev}
    </select>
</mapper>