寻路算法和避障

Junjie

2023-06-28 0f2eaeeb387850acfede95c5c061b5392dae8688

寻路算法和避障

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

@@ -317,7 +317,7 @@
        //解析json地图数据
        ArrayList arrayList = JSON.parseObject(basMap.getData(), ArrayList.class);
        NavigateMapData navigateMapData = new NavigateMapData(lev);
        List<List<MapNode>> lists = navigateMapData.filterMap(NavigationMapType.NONE.id, arrayList, lev, null);//过滤地图数据
        List<List<MapNode>> lists = navigateMapData.filterMap(NavigationMapType.NONE.id, arrayList, lev, null, null);//过滤地图数据
        return R.ok().add(lists);
    }


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

@@ -571,7 +571,7 @@
                    //数据库中也不存在地图数据，从地图文件中获取
                    //载入地图
                    NavigateMapData mapData = new NavigateMapData(i);
                    List<List<MapNode>> lists = mapData.getJsonData(-1, null);//获取完整地图(包括入库出库)
                    List<List<MapNode>> lists = mapData.getJsonData(-1, null, null);//获取完整地图(包括入库出库)

                    //存入数据库
                    basMap = new BasMap();
@@ -826,12 +826,14 @@
        }

        //计算小车起点到中点所需命令
        List<NavigateNode> calc = NavigateUtils.calc(startLocNo, locNo, mapType);
        List<NavigateNode> calc = NavigateUtils.calc(startLocNo, locNo, mapType, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), Utils.getLev(startLocNo)));
        List<ShuttleCommand> commands = new ArrayList<>();
        if (calc == null) {
            return null;
        }

        if (!Utils.checkShuttlePath(calc, shuttleThread.getSlave().getId())) {//检测穿梭车行走路径，是否存在其他小车，如有其他小车则进行调离
            return null;
        }
        List<NavigateNode> allNode = new ArrayList<>();
        allNode.addAll(calc);

@@ -881,10 +883,14 @@
        List<NavigateNode> allNode = new ArrayList<>();

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

        if (calc != null) {
            if (!Utils.checkShuttlePath(calc, shuttleThread.getSlave().getId())) {//检测穿梭车行走路径，是否存在其他小车，如有其他小车则进行调离
                return null;
            }

            allNode.addAll(calc);
            //获取分段路径
            ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
@@ -915,11 +921,13 @@
        commands.add(shuttleThread.getPalletCommand((short) 1));

        //计算小车中点到终点所需命令
        List<NavigateNode> calc2 = NavigateUtils.calc(middleLocNo, locNo, NavigationMapType.DFX.id);//小车有货，走禁用过DFX库位的地图通道
        List<NavigateNode> calc2 = NavigateUtils.calc(middleLocNo, locNo, NavigationMapType.DFX.id, null);//小车有货，走禁用过DFX库位的地图通道
        if (calc2 == null) {
            return null;
        }

        if (!Utils.checkShuttlePath(calc2, shuttleThread.getSlave().getId())) {//检测穿梭车行走路径，是否存在其他小车，如有其他小车则进行调离
            return null;
        }
        allNode.addAll(calc2);

        //获取分段路径
@@ -1006,8 +1014,11 @@
                        //判断提升机楼层是否到位，判断站点是否给出提升机到位信号
                        String locNo = wrkMast.getSourceLocNo();
                        int lev = Utils.getLev(locNo);//目标二维码所在楼层
                        int liftLev = liftProtocol.getLev().intValue();//提升机所在楼层
                        if (liftLev != lev) {
                        Short liftLev = liftProtocol.getLev();//提升机所在楼层
                        if (liftLev == null) {//提升机可能在输送线楼层
                            continue;
                        }
                        if (liftLev.intValue() != lev) {
                            continue;//提升机不在目标楼层跳过
                        }

@@ -1069,25 +1080,32 @@
                        //判断小车和库位是否在同一层
                        if (Boolean.parseBoolean(searchIdleShuttle.get("sameLay").toString())) {
                            //同一层(将小车移动到货物位置)

                            List<ShuttleCommand> commands = this.shuttleAssignCommand(currentLocNo, wrkMast.getSourceLocNo(), liftSiteLocNo, assignCommand, shuttleThread);
                            if (commands == null) {
                                //未找到路径，等待下一次
                                continue;
                            }
                            //所使用的路径进行临时解锁，用于后续计算
                            NavigateMapData navigateMapData = new NavigateMapData(currentLev);
                            navigateMapData.writeNavigateNodeToRedisMap(assignCommand.getNodes(), false);//所使用的路径进行临时解锁

                            //获取当前小车所在楼层的站点信息
                            BasDevp basDevp = basDevpService.queryByLocNo(liftSiteLocNo);
                            Short endStartCode = Short.parseShort(basDevp.getQrCodeValue());//站点二维码

                            String disLocNo = "200020" + Utils.getLev(liftSiteLocNo);//避让位置
                            LocMast locMast1 = locMastService.queryByLoc(disLocNo);
                            if (locMast1 == null) {
                                continue;//找不到库位
                            }
                            short disCode = Short.parseShort(locMast1.getQrCodeValue());
                            //任务执行完后，小车进入移开提升机口站点位置，以免坠落
                            ShuttleCommand moveCommand = shuttleThread.getMoveCommand(endStartCode, disCode, 2800, ShuttleRunDirection.BOTTOM.id, endStartCode, 2800, 1000);
                            commands.add(moveCommand);
                            //搜索一条没有小车的空巷道，并调度小车
                            int distLev = Utils.getLev(liftSiteLocNo);//避让楼层
                            String startLocNo = "180020" + Utils.getLev(liftSiteLocNo);
                            ShuttleAssignCommand moveAssignCommand = Utils.searchEmptyGroupToMoveShuttle(distLev, shuttleThread.getSlave().getId(), shuttleThread, startLocNo);
                            if (moveAssignCommand == null) {//调度小车命令为空
                                continue;
                            }
                            //所使用的路径进行临时解锁，用于后续计算
                            navigateMapData.writeNavigateNodeToRedisMap(moveAssignCommand.getNodes(), false);//所使用的路径进行临时解锁
                            commands.addAll(moveAssignCommand.getCommands());//将避让小车的命令添加
                            List<NavigateNode> nodes = assignCommand.getNodes();//将避让路径添加进节点路径中
                            nodes.addAll(moveAssignCommand.getNodes());//将避让路径添加进节点路径中
                            assignCommand.setNodes(nodes);
                            navigateMapData.writeNavigateNodeToRedisMap(nodes, true);//所使用的路径进行锁定


                            //分配目标库位
                            shuttleProtocol.setLocNo(wrkMast.getSourceLocNo());
@@ -1155,24 +1173,30 @@
                        assignCommand.setTaskMode(ShuttleTaskModeType.PAK_OUT.id.shortValue());
                        assignCommand.setSourceLocNo(liftSiteLocNo);

                        //获取当前小车所在楼层的站点信息
                        BasDevp basDevp = basDevpService.queryByLocNo(liftSiteLocNo);
                        Short endStartCode = Short.parseShort(basDevp.getQrCodeValue());//站点二维码

                        String disLocNo = "200020" + Utils.getLev(liftSiteLocNo);//避让位置
                        LocMast locMast1 = locMastService.queryByLoc(disLocNo);
                        if (locMast1 == null) {
                            continue;//找不到库位
                        }
                        short disCode = Short.parseShort(locMast1.getQrCodeValue());
                        //任务执行完后，小车进入移开提升机口站点位置，以免坠落
                        ShuttleCommand moveCommand2 = shuttleThread.getMoveCommand(endStartCode, disCode, 2800, ShuttleRunDirection.BOTTOM.id, endStartCode, 2800, 1000);

                        List<ShuttleCommand> commands = this.shuttleAssignCommand(liftSiteLocNo, wrkMast.getSourceLocNo(), liftSiteLocNo, assignCommand, shuttleThread);
                        if (commands == null) {
                            continue;//未找到路径
                        }
                        commands.add(moveCommand2);//任务执行完后，小车进入移开提升机口站点位置，以免坠落
                        //所使用的路径进行临时解锁，用于后续计算
                        NavigateMapData navigateMapData = new NavigateMapData(Utils.getLev(liftSiteLocNo));
                        navigateMapData.writeNavigateNodeToRedisMap(assignCommand.getNodes(), false);//所使用的路径进行临时解锁

                        //任务执行完后，小车进入移开提升机口站点位置，以免坠落
                        //搜索一条没有小车的空巷道，并调度小车
                        int distLev = Utils.getLev(liftSiteLocNo);//避让楼层
                        String startLocNo = "180020" + Utils.getLev(liftSiteLocNo);
                        ShuttleAssignCommand moveAssignCommand = Utils.searchEmptyGroupToMoveShuttle(distLev, shuttleThread.getSlave().getId(), shuttleThread, startLocNo);
                        if (moveAssignCommand == null) {//调度小车命令为空
                            continue;
                        }
                        //所使用的路径进行临时解锁，用于后续计算
                        navigateMapData.writeNavigateNodeToRedisMap(moveAssignCommand.getNodes(), false);//所使用的路径进行临时解锁
                        commands.addAll(moveAssignCommand.getCommands());//将避让小车的命令添加
                        List<NavigateNode> nodes = assignCommand.getNodes();//将避让路径添加进节点路径中
                        nodes.addAll(moveAssignCommand.getNodes());//将避让路径添加进节点路径中
                        assignCommand.setNodes(nodes);
                        navigateMapData.writeNavigateNodeToRedisMap(nodes, true);//所使用的路径进行锁定


                        //此时车在提升机内部，需要多下达一步指令让车移动到提升机口
                        short startCode = liftProtocol.getBarcode();//提升机内部二维码
@@ -1274,9 +1298,9 @@
                String recentLocNo = recentShuttle.getShuttleProtocol().getCurrentLocNo();

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

                //当前最近四向穿梭车到当前车子所在楼层的提升机口距离
                List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, Utils.levToOutInStaLocNo(recentLev), NavigationMapType.NORMAL.id);//搜索空闲穿梭车，使用正常通道地图
                List<NavigateNode> recentShuttlePath = NavigateUtils.calc(recentLocNo, Utils.levToOutInStaLocNo(recentLev), NavigationMapType.NORMAL.id, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), recentLev));//搜索空闲穿梭车，使用正常通道地图
                //当前穿梭车线程到当前车子所在楼层的提升机口距离
                List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, Utils.levToOutInStaLocNo(currentLev), NavigationMapType.NORMAL.id);//搜索空闲穿梭车，使用正常通道地图
                List<NavigateNode> currentShuttlePath = NavigateUtils.calc(currentLocNo, Utils.levToOutInStaLocNo(currentLev), NavigationMapType.NORMAL.id, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), currentLev));//搜索空闲穿梭车，使用正常通道地图
                if (recentShuttlePath == null || currentShuttlePath == null) {
                    continue;
                }
@@ -1740,8 +1764,6 @@
                            if (!result) {
                                throw new CoolException("更新plc站点信息失败");
                            }
                            //任务号清零
                            liftProtocol.setTaskNo((short) 0);
                            wrkMast.setWrkSts(4L);
                            break;
                        case 7://7.提升机迁移小车中 ==> 8.提升机迁移小车完成
@@ -1762,8 +1784,6 @@
                            }
                            wrkMast.setWrkSts(29L);
                            wrkMast.setWrkSts(34L);//34.出库完成，暂时先直接完成出库工作档，后续需要根据输送线给出的状态来确定34.出库完成状态
                            //任务号清零
                            liftProtocol.setTaskNo((short) 0);
                            break;
                        default:
                    }
@@ -1773,6 +1793,8 @@
                        liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE);
                        //任务指令清零
                        liftProtocol.setAssignCommand(null);
                        //任务号清零
                        liftProtocol.setTaskNo((short) 0);
                        News.info("提升机已确认且任务完成状态。提升机号={}", liftProtocol.getLiftNo());
                    } else {
                        News.error("提升机已确认且任务完成状态,复位失败，但未找到工作档。提升机号={}，工作号={}", liftProtocol.getLiftNo(), liftProtocol.getTaskNo());
@@ -1811,8 +1833,11 @@
                if (liftProtocol.getAssignCommand() != null) {
                    //设置提升机为空闲状态
                    liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE);
                    //任务号清零
                    liftProtocol.setTaskNo((short) 0);
                    //判断是否为四向穿梭车调度提升机，如是则无需清理任务号
                    if (!liftProtocol.getSecurityMk()) {
                        //任务号清零
                        liftProtocol.setTaskNo((short) 0);
                    }
                    //标记复位
                    liftProtocol.setPakMk(true);
                    //任务指令清零
@@ -2756,21 +2781,27 @@
                //判断小车是否充满电量，满电1000或电压54V以上
                if (shuttleProtocol.getBatteryPower() >= 1000 && shuttleProtocol.getCurrentVoltage() >= 54000) {
                    //充满，断开充电
                    List<ShuttleCommand> commands = new ArrayList<>();
                    ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                    assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
                    assignCommand.setTaskMode((short) 0);
                    assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
                    assignCommand.setCharge(true);
//                    List<ShuttleCommand> commands = new ArrayList<>();
//                    ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
//                    assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());
//                    assignCommand.setTaskMode((short) 0);
//                    assignCommand.setTaskNo(wrkCharge.getWrkNo().shortValue());
//                    assignCommand.setCharge(true);
//
//                    //创建充电指令
//                    ShuttleCommand command = shuttleThread.getChargeSwitchCommand((short) 2);//断开充电
//                    commands.add(command);
//
//                    //指令集分配
//                    assignCommand.setCommands(commands);
//
//                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING);

                    //创建充电指令
                    ShuttleCommand command = shuttleThread.getChargeSwitchCommand((short) 2);//断开充电
                    commands.add(command);

                    //指令集分配
                    assignCommand.setCommands(commands);

                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING);
                    //将小车移动到空闲的巷道
                    ShuttleAssignCommand assignCommand = Utils.searchEmptyGroupToMoveShuttle(Utils.getLev(shuttleProtocol.getLocNo()), shuttleProtocol.getShuttleNo().intValue(), shuttleThread, null);
                    if (assignCommand == null) {
                        continue;
                    }

                    wrkCharge.setWrkSts(60L);//60.充电任务完成
                    if (wrkChargeMapper.updateById(wrkCharge) > 0) {

 src/main/java/com/zy/asrs/utils/Utils.java

@@ -3,7 +3,28 @@
import com.alibaba.fastjson.JSON;
import com.core.common.Arith;
import com.core.common.Cools;
import com.core.common.SpringUtils;
import com.zy.asrs.entity.LocMast;
import com.zy.asrs.service.LocMastService;
import com.zy.asrs.service.impl.MainServiceImpl;
import com.zy.common.model.NavigateNode;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.common.utils.NavigateMapData;
import com.zy.common.utils.NavigatePositionConvert;
import com.zy.common.utils.NavigateUtils;
import com.zy.core.Slave;
import com.zy.core.cache.MessageQueue;
import com.zy.core.cache.SlaveConnection;
import com.zy.core.enums.ShuttleRunDirection;
import com.zy.core.enums.ShuttleTaskModeType;
import com.zy.core.enums.SlaveType;
import com.zy.core.model.ShuttleSlave;
import com.zy.core.model.Task;
import com.zy.core.model.command.ShuttleAssignCommand;
import com.zy.core.model.command.ShuttleCommand;
import com.zy.core.model.protocol.ShuttleProtocol;
import com.zy.core.properties.SlaveProperties;
import com.zy.core.thread.ShuttleThread;

import java.text.DecimalFormat;
import java.util.ArrayList;
@@ -388,4 +409,195 @@
        return result;
    }

    //获取除白名单外的指定楼层全部穿梭车xy坐标点
    public static List<int[]> getShuttlePoints(Integer whiteShuttle, Integer lev) {
        SlaveProperties slaveProperties = SpringUtils.getBean(SlaveProperties.class);
        ArrayList<int[]> list = new ArrayList<>();
        for (ShuttleSlave slave : slaveProperties.getShuttle()) {
            if (slave.getId().intValue() == whiteShuttle) {
                continue;//跳过白名单
            }

            //获取穿梭车所在节点位置
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, slave.getId());
            if (shuttleThread == null) {
                continue;
            }
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (shuttleProtocol == null) {
                continue;
            }

            if (shuttleProtocol.getLocNo() == null) {
                continue;
            }

            if (lev != Utils.getLev(shuttleProtocol.getLocNo())) {
                continue;//楼层不同
            }

            int[] xyPosition = NavigatePositionConvert.positionToXY(shuttleProtocol.getLocNo());//通过库位号获取xy坐标
            list.add(xyPosition);
        }
        return list;
    }

    //检测穿梭车行走路径，是否存在其他小车，如有其他小车则进行调离
    public static boolean checkShuttlePath(List<NavigateNode> nodes, Integer shuttleId) {
          boolean flag = false;
        int shuttleX = -1;
        int shuttleY = -1;
        int shuttleZ = -1;

        LocMastService locMastService = SpringUtils.getBean(LocMastService.class);

        if (nodes == null) {
            return false;
        }
        if (nodes.size() == 0) {
            return false;
        }
        NavigateMapData mapData = new NavigateMapData(nodes.get(0).getZ());//获取地图数据
        int[][] data = mapData.getData(-1, null, Utils.getShuttlePoints(shuttleId, nodes.get(0).getZ()));
        for (NavigateNode node : nodes) {
            int x = node.getX();
            int y = node.getY();
            if (data[x][y] == 66) {//判断该路径是否有小车
                flag = true;//存在小车
                shuttleX = x;
                shuttleY = y;
                shuttleZ = node.getZ();
                break;
            }
        }

        if (flag) {
            //检测到路径存在其他小车
            //搜索一条没有小车的空巷道

            //获取小车坐标二维码
            Short shuttlePosition = NavigatePositionConvert.xyToPosition(shuttleX, shuttleY, shuttleZ);
            LocMast shuttleLocMast = locMastService.queryByQrCode(String.valueOf(shuttlePosition));

            //获取小车线程
            SlaveProperties slaveProperties = SpringUtils.getBean(SlaveProperties.class);
            ShuttleThread currentShuttleThread = null;
            for (ShuttleSlave slave : slaveProperties.getShuttle()) {
                //获取穿梭车所在节点位置
                ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, slave.getId());
                if (shuttleThread == null) {
                    continue;
                }
                ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
                if (shuttleProtocol == null) {
                    continue;
                }

                if (shuttleProtocol.getCurrentCode().intValue() == Integer.parseInt(shuttleLocMast.getQrCodeValue())) {
                    //小车坐标和线程获取的小车坐标一致
                    currentShuttleThread = shuttleThread;
                    break;
                }
            }

            if (currentShuttleThread == null) {
                //没找到小车
                return false;
            }

            //搜索一条没有小车的空巷道，并调度小车
            ShuttleAssignCommand assignCommand = Utils.searchEmptyGroupToMoveShuttle(shuttleZ, shuttleId, currentShuttleThread, null);//shuttleId搜索时需要排除的车辆id，currentShuttleThread是需要被调度的车辆线程

            if (assignCommand == null) {
                return false;
            }

            //下发任务
            MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));

            return false;
        }

        return true;
    }

    //搜索一条没有小车的空巷道，并返回移动小车的命令
    public static ShuttleAssignCommand searchEmptyGroupToMoveShuttle(int z, Integer currentShuttleId, ShuttleThread shuttleThread, String startLocNo) {
        LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
        MainServiceImpl mainServiceImpl = SpringUtils.getBean(MainServiceImpl.class);

        if (shuttleThread == null) {
            return null;
        }
        ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
        if (shuttleProtocol == null) {
            return null;
        }

        NavigateMapData mapData = new NavigateMapData(z);//获取地图数据
        int[][] data = mapData.getData(-1, null, currentShuttleId == null ? null : Utils.getShuttlePoints(0, z));//载入全部车辆

        int distY = -1;
        int distX = -1;
        int distZ = -1;
        for (int y = 3; y <= 25; y++) {
            boolean searchFlag = true;
            for (int x = 20; x <= 23; x++) {
                if (data[x][y] < 0 || data[x][y] == 66) {
                    searchFlag = false;//该巷道有禁用节点或有小车
                    break;
                }
            }

            if (searchFlag) {
                //搜索出空巷道
                distY = y;
                distX = 20;
                distZ = z;
                break;
            }
        }

        if (distY != -1) {

            //获取避让坐标二维码
            Short distPosition = NavigatePositionConvert.xyToPosition(distX, distY, distZ);
            LocMast distLocMast = locMastService.queryByQrCode(String.valueOf(distPosition));
            if (distLocMast == null) {
                return null;
            }

            if (startLocNo == null) {
                //获取小车坐标二维码
                LocMast shuttleLocMast = locMastService.queryByQrCode(String.valueOf(shuttleProtocol.getCurrentCode()));
                if (shuttleLocMast == null) {
                    return null;
                }
                startLocNo = shuttleLocMast.getLocNo();
            }

            //创建分配命令
            ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
            assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());//四向穿梭车号
            assignCommand.setTaskNo((short) 9998);//任务号
            assignCommand.setTaskMode(ShuttleTaskModeType.MOVE_LOC_NO.id.shortValue());//移动到目标库位
            assignCommand.setSourceLocNo(startLocNo);//源库位

            //分配目标库位
            shuttleProtocol.setLocNo(distLocMast.getLocNo());
            //目标库位
            assignCommand.setLocNo(distLocMast.getLocNo());

            //小车移动到目标位置
            List<ShuttleCommand> commands = mainServiceImpl.shuttleAssignCommand(startLocNo, distLocMast.getLocNo(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
            if (commands == null) {
                return null;
            }

            assignCommand.setCommands(commands);
            return assignCommand;
        }
        return null;
    }

}

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

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

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

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

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

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

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

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

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

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

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

                return lists;
            } else {
@@ -149,9 +149,11 @@
    /**
     * 过滤地图数据
     * mapType -1=>无过滤，1=》过滤库位状态DFX，2=》过滤库位状态X
     *
     * @param whitePoints 白名单节点，不需要被过滤
     * @param shuttlePoints 穿梭车节点，需要加载进地图
     */
    public List<List<MapNode>> filterMap(Integer mapType, List arrayList, Integer lev, List<int[]> whitePoints) {
    public List<List<MapNode>> filterMap(Integer mapType, List arrayList, Integer lev, List<int[]> whitePoints, List<int[]> shuttlePoints) {
        List<List<MapNode>> lists = new ArrayList<>();

        //重建数据格式
@@ -211,6 +213,20 @@
            lists.set(row, list);
        }

        //加载车辆坐标到地图中
        if (shuttlePoints != null) {
            for (int[] points : shuttlePoints) {
                int x = points[0];
                int y = points[1];
                List<MapNode> list = lists.get(x);
                MapNode mapNode = list.get(y);
                mapNode.setValue(66);//设置为车辆代码66
                //更新list
                list.set(y, mapNode);
                lists.set(x, list);
            }
        }

        return lists;
    }

@@ -227,7 +243,7 @@

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

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

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

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

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

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

@@ -13,7 +13,7 @@
 */
public class NavigateUtils {

    public static List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType) {
    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);//结束节点
@@ -30,7 +30,7 @@
        start.setFather(null);

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

        NavigateNode res_node = solution.astarSearch(start, end);
@@ -169,7 +169,7 @@
    //获取从x点到下一点的行走距离
    public static Integer getXToNextDistance(NavigateNode xNode) {
        NavigateMapData mapData = new NavigateMapData();
        List<List<MapNode>> lists = mapData.getJsonData(NavigationMapType.NONE.id, null);
        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) {
@@ -230,7 +230,7 @@

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

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

@@ -423,9 +423,9 @@
                    //小车移动到提升机口，计算路径
                    //计算小车起点到中点所需命令
                    LocMast currentLocMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
                    List<NavigateNode> firstMastResult = NavigateUtils.calc(currentLocMast.getLocNo(), assignCommand.getSourceLocNo(), NavigationMapType.NORMAL.id);//小车到中点，处于无货状态，使用正常通道地图

                    if (firstMastResult != null) {
                    List<NavigateNode> firstMastResult = NavigateUtils.calc(currentLocMast.getLocNo(), assignCommand.getSourceLocNo(), NavigationMapType.NORMAL.id, Utils.getShuttlePoints(assignCommand.getShuttleNo().intValue(), Utils.getLev(currentLocMast.getLocNo())));//小车到中点，处于无货状态，使用正常通道地图
                    boolean checkResult = Utils.checkShuttlePath(firstMastResult, shuttleProtocol.getShuttleNo().intValue());
                    if (firstMastResult != null && checkResult) {
                        allNode.addAll(firstMastResult);//将节点进行保存
                        //获取分段路径
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(firstMastResult);
@@ -459,9 +459,9 @@
                    }

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

                    if (secMastResult != null) {
                    List<NavigateNode> secMastResult = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), NavigationMapType.DFX.id, Utils.getShuttlePoints(assignCommand.getShuttleNo().intValue(), Utils.getLev(assignCommand.getSourceLocNo())));//小车从中点到终点，处于有货状态，使用DFX地图
                    boolean checkResult2 = Utils.checkShuttlePath(secMastResult, shuttleProtocol.getShuttleNo().intValue());
                    if (secMastResult != null && checkResult2) {
                        allNode.addAll(secMastResult);//将节点进行保存
                        //获取分段路径
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(secMastResult);
@@ -544,9 +544,9 @@
                    }

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

                    if (result != null) {
                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), assignCommand.getLocNo(), NavigationMapType.NONE.id, Utils.getShuttlePoints(assignCommand.getShuttleNo().intValue(), Utils.getLev(locMast.getLocNo())));//手动命令-移动命令，使用无过滤地图
                    boolean checkResult3 = Utils.checkShuttlePath(result, shuttleProtocol.getShuttleNo().intValue());
                    if (result != null && checkResult3) {
                        //所使用的路径进行锁定禁用
                        navigateMapData = new NavigateMapData(Utils.getLev(locMast.getLocNo()));
                        navigateMapData.writeNavigateNodeToRedisMap(result, true);////所使用的路径进行锁定禁用
@@ -580,10 +580,10 @@
                    int lev = Utils.getLev(locMast1.getLocNo());//穿梭车当前高度
                    String liftSiteLocNo = Utils.levToOutInStaLocNo(lev);//当前楼层站点库位号
                    LocMast liftSitelocMast = locMastService.selectById(liftSiteLocNo);
                    List<NavigateNode> result1 = NavigateUtils.calc(locMast1.getLocNo(), liftSiteLocNo, NavigationMapType.NONE.id);//移动到提升机，使用无过滤地图

                    List<NavigateNode> result1 = NavigateUtils.calc(locMast1.getLocNo(), liftSiteLocNo, NavigationMapType.NONE.id, Utils.getShuttlePoints(assignCommand.getShuttleNo().intValue(), Utils.getLev(locMast1.getLocNo())));//移动到提升机，使用无过滤地图
                    boolean checkResult4 = Utils.checkShuttlePath(result1, shuttleProtocol.getShuttleNo().intValue());
                    Short endStartCode = null;
                    if (result1 != null) {
                    if (result1 != null && checkResult4) {
                        //所使用的路径进行锁定禁用
                        navigateMapData = new NavigateMapData(Utils.getLev(locMast1.getLocNo()));
                        navigateMapData.writeNavigateNodeToRedisMap(result1, true);////所使用的路径进行锁定禁用
@@ -656,78 +656,78 @@
            return false;
        }

        List<ShuttleCommand> errorCommands = redisCommand.getErrorCommands();
        if (errorCommands.size() > 0) {
            //优先执行该指令
            ShuttleCommand errorCommand = errorCommands.get(0);//取出指令

            if(errorCommand.getCommandWord() == 1){//正常行走命令，需要先执行完找库位命令后，再执行
                LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
                LocMast locMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
                LocMast distLocMast = locMastService.queryByQrCode(errorCommand.getStartCodeNum().toString());
                if (shuttleProtocol.getCurrentCode().equals(errorCommand.getStartCodeNum())) {
                    //起点和终点属于同一库位，无需再执行移动操作
                    errorCommands.remove(0);//移除该命令
                    redisCommand.setErrorCommands(new ArrayList<ShuttleCommand>());
                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
                    //当前步序
                    int commandStep = redisCommand.getCommandStep();
                    //步序回退
                    commandStep--;
                    redisCommand.setCommandStep(commandStep);
                    //任务数据保存到redis
                    redisUtil.set("shuttle_wrk_no_" + wrkNo, JSON.toJSONString(redisCommand));
                    shuttleProtocol.setPakMk(true);
                    return true;
                }else {
                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), distLocMast.getLocNo(), NavigationMapType.DFX.id);//错误恢复，使用DFX地图
                    if (result != null) {
                        //获取分段路径
                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(result);
                        //将每一段路径分成command指令
                        for (ArrayList<NavigateNode> nodes : data) {
                            //开始路径
                            NavigateNode startPath = nodes.get(0);
                            //目标路径
                            NavigateNode endPath = nodes.get(nodes.size() - 1);
                            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离

                            String qrCodeValue = distLocMast.getQrCodeValue();
                            errorCommand.setCommandWord((short) 1);
                            errorCommand.setStartCodeNum(shuttleProtocol.getCurrentCode());
                            errorCommand.setMiddleCodeNum((short) 1);
                            errorCommand.setDistCodeNum((short) Integer.parseInt(qrCodeValue));
                            errorCommand.setStartToDistDistance(allDistance);
                            errorCommand.setRunSpeed((short) 1000);
                            errorCommand.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
                            errorCommand.setForceMoveDistance(0);
                            errorCommand.setIOControl((short) 0);
                            errorCommand.setCommandEnd((short) 1);
                            break;
                        }
                    }
                }

                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
                //当前步序
                int commandStep = redisCommand.getCommandStep();
                //步序回退
                commandStep--;
                redisCommand.setCommandStep(commandStep);
            }

            if (!write(errorCommand)) {
                News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand));
                return false;
            } else {
                News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand));
                errorCommands.remove(0);
                redisCommand.setErrorCommands(errorCommands);
                //任务数据保存到redis
                redisUtil.set("shuttle_wrk_no_" + wrkNo, JSON.toJSONString(redisCommand));
                return true;
            }
        }
//        List<ShuttleCommand> errorCommands = redisCommand.getErrorCommands();
//        if (errorCommands.size() > 0) {
//            //优先执行该指令
//            ShuttleCommand errorCommand = errorCommands.get(0);//取出指令
//
//            if(errorCommand.getCommandWord() == 1){//正常行走命令，需要先执行完找库位命令后，再执行
//                LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
//                LocMast locMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
//                LocMast distLocMast = locMastService.queryByQrCode(errorCommand.getStartCodeNum().toString());
//                if (shuttleProtocol.getCurrentCode().equals(errorCommand.getStartCodeNum())) {
//                    //起点和终点属于同一库位，无需再执行移动操作
//                    errorCommands.remove(0);//移除该命令
//                    redisCommand.setErrorCommands(new ArrayList<ShuttleCommand>());
//                    shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
//                    //当前步序
//                    int commandStep = redisCommand.getCommandStep();
//                    //步序回退
//                    commandStep--;
//                    redisCommand.setCommandStep(commandStep);
//                    //任务数据保存到redis
//                    redisUtil.set("shuttle_wrk_no_" + wrkNo, JSON.toJSONString(redisCommand));
//                    shuttleProtocol.setPakMk(true);
//                    return true;
//                }else {
//                    List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), distLocMast.getLocNo(), NavigationMapType.DFX.id, Utils.getShuttlePoints(errorCommand.getShuttleNo().intValue()));//错误恢复，使用DFX地图
//                    if (result != null) {
//                        //获取分段路径
//                        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(result);
//                        //将每一段路径分成command指令
//                        for (ArrayList<NavigateNode> nodes : data) {
//                            //开始路径
//                            NavigateNode startPath = nodes.get(0);
//                            //目标路径
//                            NavigateNode endPath = nodes.get(nodes.size() - 1);
//                            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
//
//                            String qrCodeValue = distLocMast.getQrCodeValue();
//                            errorCommand.setCommandWord((short) 1);
//                            errorCommand.setStartCodeNum(shuttleProtocol.getCurrentCode());
//                            errorCommand.setMiddleCodeNum((short) 1);
//                            errorCommand.setDistCodeNum((short) Integer.parseInt(qrCodeValue));
//                            errorCommand.setStartToDistDistance(allDistance);
//                            errorCommand.setRunSpeed((short) 1000);
//                            errorCommand.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
//                            errorCommand.setForceMoveDistance(0);
//                            errorCommand.setIOControl((short) 0);
//                            errorCommand.setCommandEnd((short) 1);
//                            break;
//                        }
//                    }
//                }
//
//                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
//                //当前步序
//                int commandStep = redisCommand.getCommandStep();
//                //步序回退
//                commandStep--;
//                redisCommand.setCommandStep(commandStep);
//            }
//
//            if (!write(errorCommand)) {
//                News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand));
//                return false;
//            } else {
//                News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand));
//                errorCommands.remove(0);
//                redisCommand.setErrorCommands(errorCommands);
//                //任务数据保存到redis
//                redisUtil.set("shuttle_wrk_no_" + wrkNo, JSON.toJSONString(redisCommand));
//                return true;
//            }
//        }

        List<ShuttleCommand> commands = redisCommand.getAssignCommand().getCommands();
        //当前步序
@@ -817,7 +817,7 @@
                if (!liftProtocol.isIdleNoTask()) {
                    return false;//提升机忙，禁止下发命令
                }
                if (liftProtocol.getTaskNo().intValue() != wrkNo) {
                if (liftProtocol.getTaskNo().intValue() != 0 && liftProtocol.getTaskNo().intValue() != wrkNo) {
                    //提升机工作号和当前工作不相同，禁止下发命令
                    return false;
                }