行走距离

Junjie

2023-07-24 56a19be6c03e729b112bf8197960d7cc4d9d498e

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

@@ -261,9 +261,9 @@
                            News.error("请求接口失败！！！url：{}；request：{}；response：{}", wmsUrl + "/rpc/pakin/loc/v1", JSON.toJSONString(param), response);
                        } else if (code == 700) {
                            staProtocol.setWorkNo((short) 32002);
                            staProtocol.setStaNo(inSta.getBackSta().shortValue());
                            staProtocol.setRollback102(1);//102站回退信号
                            devpThread.setPakMk(staProtocol.getSiteId(), false);
                            MessageQueue.offer(SlaveType.Devp, devp.getId(), new Task(2, staProtocol));
                            MessageQueue.offer(SlaveType.Devp, devp.getId(), new Task(5, staProtocol));

                            // led 异常显示
                            if (ledThread != null) {
@@ -935,16 +935,16 @@
            Integer middleToDistDistance = null;//计算中间点到目标点行走距离
            if (nodes.size() > 10) {//中段码传倒数第三个
                //中间路径
                middlePath = nodes.get(nodes.size() - 3);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离
            } else if (nodes.size() > 5) {//中段码传倒数第二个
                //中间路径
                middlePath = nodes.get(nodes.size() - 2);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            } else if (nodes.size() > 5) {//中段码传倒数第二个
                //中间路径
                middlePath = nodes.get(nodes.size() - 1);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            }

            //目标路径
@@ -1005,16 +1005,16 @@
                Integer middleToDistDistance = null;//计算中间点到目标点行走距离
                if (nodes.size() > 10) {//中段码传倒数第三个
                    //中间路径
                    middlePath = nodes.get(nodes.size() - 3);
                    //通过xy坐标小车二维码
                    middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                    middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离
                } else if (nodes.size() > 5) {//中段码传倒数第二个
                    //中间路径
                    middlePath = nodes.get(nodes.size() - 2);
                    //通过xy坐标小车二维码
                    middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                    middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离
                    middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
                } else if (nodes.size() > 5) {//中段码传倒数第二个
                    //中间路径
                    middlePath = nodes.get(nodes.size() - 1);
                    //通过xy坐标小车二维码
                    middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                    middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
                }

                //目标路径
@@ -1057,16 +1057,16 @@
            Integer middleToDistDistance = null;//计算中间点到目标点行走距离
            if (nodes.size() > 10) {//中段码传倒数第三个
                //中间路径
                middlePath = nodes.get(nodes.size() - 3);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离
            } else if (nodes.size() > 5) {//中段码传倒数第二个
                //中间路径
                middlePath = nodes.get(nodes.size() - 2);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes);//计算中间点到目标点行走距离
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            } else if (nodes.size() > 5) {//中段码传倒数第二个
                //中间路径
                middlePath = nodes.get(nodes.size() - 1);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            }

            //目标路径
@@ -1343,12 +1343,15 @@

        //判断其他空闲穿梭车是否离任务最近
        String distLocNo = null;//目标地点，入库=》提升机口，出库=》货物库位号
        if (wrkMast.getIoType() < 101) {
        if (wrkMast.getIoType() == 1) {
            //入库
            distLocNo = Utils.levToOutInStaLocNo(lev);
        }else {
        }else if(wrkMast.getIoType() == 101){
            //出库
            distLocNo = locNo;
        } else if (wrkMast.getIoType() == 11) {
            //库位移转
            distLocNo = wrkMast.getSourceLocNo();
        }

        //判断当前任务所在楼层是否有其他任务已经分配了小车，如有则直接用该小车(一层楼仅分配一台车)
@@ -1827,12 +1830,13 @@
                    LiftCommand command2 = liftThread.getLiftTurnCommand(liftProtocol.getLiftNo(), liftProtocol.getTaskNo(), 4);
                    command2.setOperaStaNo(staProtocol.getSiteId().shortValue());//输送线操作站点号
                    command2.setRotationDire(2);//给输送线下发链条转动信号，反转
                    command2.setDevpId(devpId);//输送线iD
                    command2.setDevpId(devpId);//输送线ID
                    command2.setStaNo((short) 104);//写入出库目标站104
                    commands.add(command2);//将命令添加进list

                    //提升机前往出库口，输送线楼层
                    LiftCommand command3 = liftThread.getLiftUpDownCommand(liftProtocol.getLiftNo(), liftProtocol.getTaskNo(), LiftLevType.TWO.lev);
                    command3.setDevpId(devpId);//输送线ID
                    commands.add(command3);//将命令添加进list

                    //提升机到达指定楼层，输送线将货物移出去(反转)
@@ -1840,12 +1844,12 @@
                    LiftCommand command4 = liftThread.getLiftTurnCommand(liftProtocol.getLiftNo(), liftProtocol.getTaskNo(), 2);
                    command4.setOperaStaNo((short) 102);//操作102站
                    command4.setRotationDire(2);//给输送线下发链条转动信号，反转
                    command4.setDevpId(devpId);
                    command4.setDevpId(devpId);//输送线ID
                    commands.add(command4);//将命令添加进list

                    //提升机链条执行完毕后，给102站写入资料
                    LiftCommand command5 = liftThread.getResetCommand();
                    command5.setDevpId(devpId);//输送线iD
                    command5.setDevpId(devpId);//输送线ID
                    command5.setOperaStaNo((short) 102);//操作102站
                    command5.setStaNo((short) 104);//写入出库目标站104
                    command5.setRotationDire(0);//链条转动停止
@@ -2012,285 +2016,229 @@
    }

    /**
     * 当前库位组是否存在空闲小车
     * 库位移转
     */
    public Integer hasCar(String locNo) {
        for (SteSlave ste : slaveProperties.getSte()) {
            // 获取堆垛机信息
            SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, ste.getId());
            SteProtocol steProtocol = steThread.getSteProtocol();
            BasSte basSte = basSteService.selectById(ste.getId());
            if (Cools.isEmpty(steProtocol, basSte)) { continue; }
            if (Utils.getGroupRow(locNo, true).equals(Utils.getGroupRow(basSte.getRow(), true))
                    && basSte.getBay() == Utils.getBay(locNo)
                    && basSte.getLev() == Utils.getLev(locNo)) {
                return ste.getId();
            }
        }
        return null;
    }

    /**
     * 当前库位组是否存在其他小车
     */
    public Integer existOtherSte(String locNo, Integer steNo) {
        Integer otherSteNo = null;
        for (SteSlave ste : slaveProperties.getSte()) {
            if (ste.getId().equals(steNo)) { continue; }
            // 获取堆垛机信息
            SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, ste.getId());
            SteProtocol steProtocol = steThread.getSteProtocol();
            if (steProtocol == null) { continue; }
            if (steProtocol.isEnable()) {
                BasSte basSte = basSteService.selectById(ste.getId());
                if (basSte != null) {
                    if ((Utils.getGroupRow(locNo, true).equals(Utils.getGroupRow(basSte.getRow(), true))
                            && basSte.getBay() == Utils.getBay(locNo)
                            && basSte.getLev() == Utils.getLev(locNo))
                        ||
                        (Utils.getGroupRow(locNo, true).equals(Utils.getGroupRow(steProtocol.getRow().intValue(), true))
                                && steProtocol.getBay() == Utils.getBay(locNo)
                                && steProtocol.getLev() == Utils.getLev(locNo))
                    ) {
                        otherSteNo = ste.getId();
                        break;
                    }
                }
            }
        }
        return otherSteNo;
    }

    /**
     * 当前库位组是否存在空闲小车
     */
    public Integer hasCarOfIdle(String locNo) {
        for (SteSlave ste : slaveProperties.getSte()) {
            // 获取堆垛机信息
            SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, ste.getId());
            SteProtocol steProtocol = steThread.getSteProtocol();
            if (steProtocol == null) { continue; }
            if (steProtocol.isIdle()) {
                if (Utils.getGroupRow(locNo, true).equals(Utils.getGroupRow(steProtocol.getRow().intValue(), true))
                        && steProtocol.getBay() == Utils.getBay(locNo)
                        && steProtocol.getLev() == Utils.getLev(locNo)) {
                    return steProtocol.getSteNo().intValue();
                }
            }
        }
        return null;
    }

    /**
     * 查找当前库位最适合的穿梭车来作业
     */
    public SteThread queryIdleCar(WrkMast wrkMast) {
        List<BasSte> basStes = basSteService.selectList(new EntityWrapper<>());
        int val = 0;
        SteThread result = null;
        for (BasSte basSte : basStes) {
            // 获取穿梭车信息
            SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, basSte.getSteNo());
            SteProtocol steProtocol = steThread.getSteProtocol();
            if (steProtocol == null) { continue; }
            if (!steProtocol.isIdle()) { continue; }
//            if (steProtocol.getRow() == 1) { continue; }
            String locNo = wrkMast.getWrkSts() < 10 ? wrkMast.getLocNo() : wrkMast.getSourceLocNo();
            // 如果在同一个库位组
            if (Utils.getGroupRow(locNo, true).equals(Utils.getGroupRow(steProtocol.getRow().intValue(), true))
                    && steProtocol.getBay().intValue() == Utils.getBay(locNo)
                    && steProtocol.getLev().intValue() == Utils.getLev(locNo)) {
                return steThread;
            }
            LocMast locMast = locMastService.selectById(locNo);
            int lev = locMast.getLev1();
            int bay = locMast.getBay1();
            int levAbs = Math.abs(lev - steProtocol.getLev());
            int bayAbs = Math.abs(bay - steProtocol.getBay());
            if (val == 0) {
                val = levAbs + bayAbs;
                result = steThread;
            } else {
                if ((levAbs + bayAbs) < val) {
                    val = levAbs + bayAbs;
                    result = steThread;
                }
            }
        }
        return result;
    }

    public SteThread queryIdleCar(CrnProtocol crnProtocol) {
        List<BasSte> basStes = basSteService.selectList(new EntityWrapper<>());
        int val = 0;
        SteThread result = null;
        for (BasSte basSte : basStes) {
            // 获取穿梭车信息
            SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, basSte.getSteNo());
            SteProtocol steProtocol = steThread.getSteProtocol();
            if (steProtocol == null) { continue; }
            if (!steProtocol.isIdle()) { continue; }
            if (steProtocol.getRow() == 1) { continue;}
            int lev = crnProtocol.getLevel();
            int bay = crnProtocol.getBay();
            int levAbs = Math.abs(lev - steProtocol.getLev());
            int bayAbs = Math.abs(bay - steProtocol.getBay());
            if (val == 0) {
                val = levAbs + bayAbs;
                result = steThread;
            } else {
                if ((levAbs + bayAbs) < val) {
                    val = levAbs + bayAbs;
                    result = steThread;
                }
            }
        }
        return result;
    }

    /**
     * 让小车行走至待机位
     */
    public void letCarBeWaiting(WrkMast wrkMast, Integer steNo, String locNo) {
        // 获取穿梭车信息
        SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, steNo);
        SteProtocol steProtocol = steThread.getSteProtocol();
        if (steProtocol == null) { return; }
        if (steProtocol.isIdle()) {

            // 如果处于待机位
            if (steProtocol.locaType.equals(SteLocaType.A_WAITING) || steProtocol.locaType.equals(SteLocaType.B_WAITING)) {
                // 修改工作档状态 绑定穿梭车
                wrkMast.setWrkSts(3L);
                wrkMast.setSteNo(steNo);
                wrkMast.setModiTime(new Date());
                if (wrkMastMapper.updateById(wrkMast) == 0) {
                    News.error("修改工作档状态 绑定穿梭车 失败！！，工作号={}", wrkMast.getWrkNo());
                }
            } else {
                // 命令下发区 --------------------------------------------------------------------------
                SteCommand steCommand = new SteCommand();
                steCommand.setSteNo(steNo); // 穿梭车编号
                steCommand.setTaskNo(wrkMast.getWrkNo()); // 工作号
                steCommand.setTaskMode(SteTaskModeType.findWaiting(steProtocol.getRow().intValue())); // 任务模式:  行走至待机位

                steCommand.setRow(Utils.getGroupRow(steProtocol.getRow().intValue(), true).shortValue());
                steCommand.setBay(steProtocol.getBay());
                steCommand.setLev(steProtocol.getLev());

                if (!MessageQueue.offer(SlaveType.Ste, steNo, new Task(2, steCommand))) {
                    News.error("穿梭车命令下发失败，穿梭车号={}，任务数据={}", steNo, JSON.toJSON(steCommand));
                } else {
                    // 修改工作档状态 绑定穿梭车
                    wrkMast.setSteNo(steNo);
                    wrkMast.setModiTime(new Date());
                    if (wrkMastMapper.updateById(wrkMast) == 0) {
                        News.error("修改工作档状态 绑定穿梭车 失败！！，工作号={}", wrkMast.getWrkNo());
                    }
                }
            }
        }
    }

    public void letCarBeWaiting(WrkCharge wrkCharge, Integer steNo) {
        // 获取穿梭车信息
        SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, steNo);
        SteProtocol steProtocol = steThread.getSteProtocol();
        if (steProtocol == null) { return; }

        if (!basSteService.updatePakMk(steNo, "Y")) {
            News.error("修改穿梭车作业状态 失败！！，穿梭车={}", steNo);
    public synchronized void locToLocExecute() {
        //获取出入库工作档
        List<WrkMast> wrkMasts = wrkMastMapper.selectInOutWrkMast();
        if (wrkMasts.size() > 0) {
            //有出入库任务，必须等待任务执行完毕再执行库位移转
            return;
        }

        // 命令下发区 --------------------------------------------------------------------------
        SteCommand steCommand = new SteCommand();
        steCommand.setSteNo(steNo); // 穿梭车编号
        steCommand.setTaskNo(wrkCharge.getWrkNo()); // 工作号
        steCommand.setTaskMode(SteTaskModeType.findOriginByLoc(steProtocol.getRow().intValue())); // 任务模式:  去近点 等待堆垛机叉取
        //查询库位移转工作档
        List<WrkMast> wrkMasts1 = wrkMastMapper.selectLocToLocWrkMast();
        for (WrkMast wrkMast : wrkMasts1) {

        steCommand.setRow(Utils.getGroupRow(steProtocol.getRow().intValue(), true).shortValue());
        steCommand.setBay(steProtocol.getBay());
        steCommand.setLev(steProtocol.getLev());

        if (!MessageQueue.offer(SlaveType.Ste, steNo, new Task(2, steCommand))) {
            News.error("穿梭车命令下发失败，穿梭车号={}，任务数据={}", steNo, JSON.toJSON(steCommand));
        } else {
            // 修改工作档状态 绑定穿梭车
            wrkCharge.setSteNo(steNo);
            wrkCharge.setModiTime(new Date());
            if (!wrkChargeService.updateById(wrkCharge)) {
                News.error("修改工作档状态 绑定穿梭车 失败！！，工作号={}", wrkCharge.getWrkNo());
            boolean step1 = this.locToLocExecuteStep1(wrkMast);//绑定小车
            if (!step1) {
                continue;
            }

            boolean step2 = this.locToLocExecuteStep2(wrkMast);//调度小车到目标楼层
            if (!step2) {
                continue;
            }

            boolean step3 = this.locToLocExecuteStep3(wrkMast);//同楼层库位移转
            if (!step3) {
                continue;
            }

        }
    }

    /**
     * 让小车从 远点 ====>> 移动到近点
     *  等待堆垛机搬运
     * 绑定小车并调度车
     * 如需主方法执行continue，请返回false
     * ps:返回值true并不代表该方法执行成功，返回值仅做标记用于主方法是否执行continue
     */
    public void letCarBeReady(WrkMast wrkMast, Integer steNo, String locNo) {
        // 获取穿梭车信息
        SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, steNo);
        SteProtocol steProtocol = steThread.getSteProtocol();
        if (steProtocol == null) { return; }
        if (steProtocol.isIdle()) {

            // 命令下发区 --------------------------------------------------------------------------
            SteCommand steCommand = new SteCommand();
            steCommand.setSteNo(steNo); // 穿梭车编号
            steCommand.setTaskNo(wrkMast.getWrkNo()); // 工作号
            steCommand.setTaskMode(SteTaskModeType.findOriginByLoc(steProtocol)); // 任务模式:  去近点 等待堆垛机叉取

            steCommand.setRow(Utils.getGroupRow(steProtocol.getRow().intValue(), true).shortValue());
            steCommand.setBay(steProtocol.getBay());
            steCommand.setLev(steProtocol.getLev());

            if (!MessageQueue.offer(SlaveType.Ste, steNo, new Task(2, steCommand))) {
                News.error("穿梭车命令下发失败，穿梭车号={}，任务数据={}", steNo, JSON.toJSON(steCommand));
            } else {
                // 修改工作档状态 绑定穿梭车
                wrkMast.setSteNo(steNo);
                wrkMast.setModiTime(new Date());
                if (wrkMastMapper.updateById(wrkMast) == 0) {
                    News.error("修改工作档状态 绑定穿梭车 失败！！，工作号={}", wrkMast.getWrkNo());
                }
    private boolean locToLocExecuteStep1(WrkMast wrkMast) {
        if (wrkMast.getShuttleNo() == null) {//给库位移转绑定穿梭车号
            //寻找最近且空闲的四向穿梭车
            HashMap<String,Object> searchIdleShuttle = this.searchIdleShuttle(wrkMast);
            ShuttleThread shuttleThread = (ShuttleThread) searchIdleShuttle.get("result");
            if (shuttleThread == null) {
                //没有找到空闲穿梭车
                return false;
            }
            wrkMast.setShuttleNo(shuttleThread.getSlave().getId());//给工作档分配四向穿梭车号
            wrkMastMapper.updateById(wrkMast);
        }
        return true;
    }

    /**
     * 让小车从 远点 ====>> 移动到近点
     *  等待堆垛机搬运
     * 调度小车到目标楼层
     * 如需主方法执行continue，请返回false
     * ps:返回值true并不代表该方法执行成功，返回值仅做标记用于主方法是否执行continue
     */
    public void letCarBeReady(WrkCharge wrkCharge, Integer steNo) {
        // 获取穿梭车信息
        SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, steNo);
        SteProtocol steProtocol = steThread.getSteProtocol();
        if (steProtocol == null) { return; }
        if (steProtocol.isIdle()) {
    private boolean locToLocExecuteStep2(WrkMast wrkMast) {
        if (wrkMast.getWrkSts() == 1 && wrkMast.getShuttleNo() != null) {
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, wrkMast.getShuttleNo());
            ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
            if (!shuttleProtocol.isIdle(wrkMast.getWrkNo().shortValue())) {
                return false;//小车处于不空闲状态
            }

            // 命令下发区 --------------------------------------------------------------------------
            SteCommand steCommand = new SteCommand();
            steCommand.setSteNo(steNo); // 穿梭车编号
            steCommand.setTaskNo(wrkCharge.getWrkNo()); // 工作号
            steCommand.setTaskMode(SteTaskModeType.findOriginByLoc(steProtocol)); // 任务模式:  去近点 等待堆垛机叉取
            String currentLocNo = shuttleProtocol.getCurrentLocNo();//小车当前库位号
            int shuttleLev = Utils.getLev(currentLocNo);//小车所在楼层

            steCommand.setRow(Utils.getGroupRow(steProtocol.getRow().intValue(), true).shortValue());
            steCommand.setBay(steProtocol.getBay());
            steCommand.setLev(steProtocol.getLev());
            LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1);
            LiftProtocol liftProtocol = liftThread.getLiftProtocol();

            if (!MessageQueue.offer(SlaveType.Ste, steNo, new Task(2, steCommand))) {
                News.error("穿梭车命令下发失败，穿梭车号={}，任务数据={}", steNo, JSON.toJSON(steCommand));
            } else {
                // 修改工作档状态 绑定穿梭车
                wrkCharge.setSteNo(steNo);
                wrkCharge.setModiTime(new Date());
                if (!wrkChargeService.updateById(wrkCharge)) {
                    News.error("修改工作档状态 绑定穿梭车 失败！！，工作号={}", wrkCharge.getWrkNo());
            //判断小车是否再目标楼层
            if (shuttleLev != Utils.getLev(wrkMast.getLocNo())) {
                //小车和目标不在同一楼层

                //提升机口站点库位号
                String liftSiteLocNo = Utils.levToOutInStaLocNo(shuttleLev);

                //创建分配命令
                ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
                assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());//四向穿梭车号
                assignCommand.setTaskNo(wrkMast.getWrkNo().shortValue());//任务号
                assignCommand.setTaskMode(ShuttleTaskModeType.PAK_IN.id.shortValue());//入出库模式
                assignCommand.setSourceLocNo(currentLocNo);//源库位(小车当前位置)

                //小车移动到提升机口，计算路径
                List<ShuttleCommand> commands = this.shuttleAssignCommand(shuttleProtocol.getLocNo(), liftSiteLocNo, NavigationMapType.NONE.id, assignCommand, shuttleThread);
                if (commands == null) {
                    return false;//未找到路径
                }

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

                //增加移动进提升机命令
                ShuttleCommand moveCommand = shuttleThread.getMoveCommand(endStartCode, liftProtocol.getBarcode(), 1600, ShuttleRunDirection.TOP.id, null, null, 500);
                commands.add(moveCommand);

                //分配目标库位
                shuttleProtocol.setLocNo(liftSiteLocNo);
                //目标库位
                assignCommand.setLocNo(liftSiteLocNo);
                assignCommand.setCommands(commands);
                wrkMast.setWrkSts(5L);//小车迁移状态

                if (wrkMastMapper.updateById(wrkMast) > 0) {
                    //下发任务
                    MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
                }
            }
        }
        return true;
    }

    /**
     * 同楼层库位移转
     * 如需主方法执行continue，请返回false
     * ps:返回值true并不代表该方法执行成功，返回值仅做标记用于主方法是否执行continue
     */
    private boolean locToLocExecuteStep3(WrkMast wrkMast) {
        if (wrkMast.getShuttleNo() == null) {
            return false;
        }

        ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, wrkMast.getShuttleNo());
        ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
        if (!shuttleProtocol.isIdle(wrkMast.getWrkNo().shortValue())) {
            return false;//小车处于不空闲状态
        }

        LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1);
        LiftProtocol liftProtocol = liftThread.getLiftProtocol();

        DevpThread devpThread = null;
        for (DevpSlave devp : slaveProperties.getDevp()){
            // 获取入库站信息
            devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, devp.getId());
        }

        //判断小车是否在工作档任务目标楼层
        String currentLocNo = shuttleProtocol.getCurrentLocNo();//小车当前库位号
        int shuttleLev = Utils.getLev(currentLocNo);//小车所在楼层
        if (shuttleLev != Utils.getLev(wrkMast.getLocNo())) {
            return false;//不在同一楼层
        }

        if (wrkMast.getWrkSts() == 1 || wrkMast.getWrkSts() == 8) {
            //调度小车执行同楼层移库任务

            //创建分配命令
            ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
            assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo());//四向穿梭车号
            assignCommand.setTaskNo(wrkMast.getWrkNo().shortValue());//任务号
            assignCommand.setTaskMode(ShuttleTaskModeType.PAK_IN.id.shortValue());//入出库模式
            assignCommand.setSourceLocNo(currentLocNo);//源库位(小车当前位置)

            List<ShuttleCommand> commands = new ArrayList<>();

            if (wrkMast.getWrkSts() == 8) {//8.提升机迁移小车完成，需要将小车移出提升机
                //判断提升机是否空闲
                if (!liftProtocol.isIdleNoTask()) {
                    return false;//提升机忙
                }
                //判断提升机任务号和当前工作档任务号是否一致
                if (liftProtocol.getTaskNo().intValue() != 0 && liftProtocol.getTaskNo().intValue() != wrkMast.getWrkNo()) {
                    return false;
                }

                //判断提升机楼层是否到位，判断站点是否给出提升机到位信号
                String locNo = wrkMast.getLocNo();
                int lev = Utils.getLev(locNo);//目标二维码所在楼层
                int liftLev = liftProtocol.getLev().intValue();//提升机所在楼层
                if (liftLev != lev) {
                    return false;//提升机不在目标楼层跳过
                }

                Integer staNo = Utils.levToOutInStaNo(lev >= 2 ? lev + 1 : lev);
                //获取目标站信息
                StaProtocol staProtocol1 = devpThread.getStation().get(staNo);
                if (staProtocol1 == null) {
                    return false;//站点信息不存在
                }
                if (!staProtocol1.isLiftArrival()) {
                    return false;//站点提升机到位信号false
                }

                BasDevp basDevp = basDevpService.selectById(staNo);
                short startCode = liftProtocol.getBarcode();//提升机内部二维码
                Short distCode = Short.parseShort(basDevp.getQrCodeValue());//提升机口站点二维码
                Short runDirection = ShuttleRunDirection.BOTTOM.id;//运行方向
                //获取命令
                ShuttleCommand moveCommand = shuttleThread.getMoveCommand(startCode, distCode, 1600, runDirection, null, null, 500);
                commands.add(0, moveCommand);//将该指令添加到队头

                currentLocNo = basDevp.getLocNo();//使用输送站点口作为起点坐标
            }

            //直接计算车到源库位到目标库位路径
            List<ShuttleCommand> commands1 = this.shuttleAssignCommand(currentLocNo, wrkMast.getSourceLocNo(), wrkMast.getLocNo(), assignCommand, shuttleThread);
            if (commands1 == null) {
                return false;//找不到路径等待下一次
            }
            commands.addAll(commands1);

            //分配任务号
            shuttleProtocol.setTaskNo(wrkMast.getWrkNo().shortValue());
            //分配源库位
            shuttleProtocol.setSourceLocNo(wrkMast.getSourceLocNo());

            assignCommand.setCommands(commands);
            //分配目标库位
            shuttleProtocol.setLocNo(wrkMast.getLocNo());
            //目标库位
            assignCommand.setLocNo(wrkMast.getLocNo());
            wrkMast.setWrkSts(9L);//小车入库中
            if (wrkMastMapper.updateById(wrkMast) > 0) {
                //下发任务
                MessageQueue.offer(SlaveType.Shuttle, assignCommand.getShuttleNo().intValue(), new Task(3, assignCommand));
            }
        }

        return true;
    }

    /**
@@ -2983,183 +2931,7 @@

    }

    public List<String> crn2DemoLocs = new ArrayList<String>(); public String crn2LastLoc = "";
    public synchronized void demo() {
        if (Cools.isEmpty(crn2DemoLocs)) {
            crn2DemoLocs = locMastService.getDemoNextLoc(2);
        }
        for (CrnSlave crn : slaveProperties.getCrn()) {
            if (!crn.getDemo()) {
                continue;
            }
            // 必须为演示状态
            CrnThread crnThread = (CrnThread) SlaveConnection.get(SlaveType.Crn, crn.getId());
            CrnProtocol crnProtocol = crnThread.getCrnProtocol();
            if (crnProtocol == null) {
                continue;
            }

            // 只有当堆垛机空闲 并且 无任务时才继续执行
            if (crnProtocol.getStatusType() == CrnStatusType.IDLE && crnProtocol.getTaskNo() == 0 && crnProtocol.getModeType() == CrnModeType.AUTO) {

                // filter
                if (wrkChargeService.selectCount(new EntityWrapper<WrkCharge>()
                        .eq("crn_no", crn.getId()).between("wrk_sts", 31, 36)) > 0) {
                    continue;
                }

                SteThread steThread = queryIdleCar(crnProtocol);
                if (Cools.isEmpty(steThread)) {
                    News.warn("没有空闲的小车进行演示功能");
                    continue;
                }
                int steNo = steThread.getSlave().getId();
                SteProtocol steProtocol = steThread.getSteProtocol();
                BasSte basSte = basSteService.selectById(steNo);
                if (Cools.isEmpty(steProtocol, basSte)) { continue; }

                // 只有当穿梭车空闲 并且 无任务时才继续执行
                if (steProtocol.isIdle()) {

                    String locNo = null;
                    Iterator<String> iterator = crn2DemoLocs.iterator();
                    while (iterator.hasNext()) {
                        String next = iterator.next();
                        if (!Cools.isEmpty(this.hasCar(next))) { continue; }
                        String lastLoc = crn2LastLoc;
                        if (!Cools.isEmpty(lastLoc)) {
                            if (!lastLoc.substring(2, 7).equals(next.substring(2, 7))
                                    || !Utils.getGroupRow(lastLoc, true).equals(Utils.getGroupRow(next, true))) {
                                locNo = next;
                                iterator.remove();
                                break;
                            } else {
                                iterator.remove();
                            }
                        } else {
                            locNo = next;
                            iterator.remove();
                            break;
                        }

                    }
                    if (!Cools.isEmpty(locNo)) {
                        crn2LastLoc = locNo;
                    }

                    News.info("{}号堆垛机对{}库位进行演示", crn.getId(), locNo);

                    String sourceLocNo = Utils.getLocNo(steProtocol.getRow(), steProtocol.getBay(), steProtocol.getLev());

                    WrkCharge wrkCharge = new WrkCharge();
                    wrkCharge.setCrnNo(crn.getId());
                    wrkCharge.setSteNo(steNo);
                    wrkCharge.setWrkNo(commonService.getChargeWorkNo(5));
                    wrkCharge.setWrkSts(31L);   // 31.生成演示ID
                    wrkCharge.setIoPri((double) 10);
                    wrkCharge.setSourceLocNo(sourceLocNo);
                    wrkCharge.setLocNo(locNo);
                    wrkCharge.setMemo("demo");
                    if (!wrkChargeService.insert(wrkCharge)) {
                        News.error("生成演示任务失败，堆垛机号：{}，小车：{}，源库位：{}，目标库位：{}", crn.getId(), steNo, sourceLocNo, locNo);
                    }
                }
            }
        }
    }

    /**
     * 演示取货
     */
    private void steMoveDemo(CrnSlave slave, CrnProtocol crnProtocol) {
        // 获取工作档信息
        WrkCharge wrkCharge = wrkChargeService.selectOne(new EntityWrapper<WrkCharge>().eq("crn_no", slave.getId())
                .in("wrk_sts", 31, 33, 35));
        if (null == wrkCharge) {
            return;
        }
        SteThread steThread = (SteThread) SlaveConnection.get(SlaveType.Ste, wrkCharge.getSteNo());
        SteProtocol steProtocol = steThread.getSteProtocol();
        if (null == steProtocol) { return; }
        if (steProtocol.isIdle()) {
            // 31.生成演示ID
            if (wrkCharge.getWrkSts() == 31L) {
                this.letCarBeReady(wrkCharge, steProtocol.getSteNo().intValue());
                wrkCharge.setWrkSts(32L);
                wrkCharge.setModiTime(new Date());
                if (!wrkChargeService.updateById(wrkCharge)) {
                    News.error("修改演示任务{}工作档 31.生成演示ID ==>> 32.小车走行 失败!", wrkCharge.getWrkNo());
                }
            // 33.小车待搬
            } else if (wrkCharge.getWrkSts() == 33L) {
                LocMast locMast = locMastService.selectById(wrkCharge.getLocNo());
                // 堆垛机命令下发区 --------------------------------------------------------------------------
                CrnCommand crnCommand = new CrnCommand();
                crnCommand.setCrnNo(crnProtocol.getCrnNo()); // 堆垛机编号
                crnCommand.setTaskNo(wrkCharge.getWrkNo().shortValue()); // 工作号
                crnCommand.setAckFinish((short) 0);  // 任务完成确认位
                crnCommand.setTaskMode(CrnTaskModeType.STE_MOVE); // 任务模式:  库位移转
                crnCommand.setSourcePosX(Utils.getGroupRow(steProtocol.getRow().intValue(), true).shortValue());     // 源库位排
                crnCommand.setSourcePosY(steProtocol.getBay());     // 源库位列
                crnCommand.setSourcePosZ(steProtocol.getLev());     // 源库位层
                crnCommand.setDestinationPosX(Utils.getGroupRow(locMast.getLocNo(), true).shortValue());     // 目标库位排
                crnCommand.setDestinationPosY(locMast.getBay1().shortValue());     // 目标库位列
                crnCommand.setDestinationPosZ(locMast.getLev1().shortValue());     // 目标库位层
                if (!MessageQueue.offer(SlaveType.Crn, wrkCharge.getCrnNo(), new Task(2, crnCommand))) {
                    News.error("堆垛机命令下发失败，堆垛机号={}，任务数据={}", wrkCharge.getCrnNo(), JSON.toJSON(crnCommand));
                } else {
                    // 修改穿梭车运行中排列层
                    steThread.modifyPos(Utils.getGroupRow(locMast.getLocNo(), true), locMast.getBay1(), locMast.getLev1());
                    // 修改工作档状态 33.小车待搬 => 34.吊车搬运
                    Date now = new Date();
                    wrkCharge.setWrkSts(34L);
                    wrkCharge.setCrnStrTime(now);
                    wrkCharge.setModiTime(now);
                    if (!wrkChargeService.updateById(wrkCharge)) {
                        News.error("修改演示任务{}工作档 33.小车待搬 => 34.吊车搬运 失败!", wrkCharge.getWrkNo());
                    }
                }
                // 35.小车就绪
            } else if (wrkCharge.getWrkSts() == 35L) {

                int steNo = steProtocol.getSteNo().intValue();
                if (!basSteService.updatePakMk(steNo, "Y")) {
                    News.error("修改穿梭车作业状态 失败！！，穿梭车={}", steNo);
                    return;
                }

                // 命令下发区 --------------------------------------------------------------------------
                SteCommand steCommand = new SteCommand();
                steCommand.setSteNo(steNo); // 穿梭车编号
                steCommand.setTaskNo(wrkCharge.getWrkNo()); // 工作号
                SteTaskModeType originByLoc = SteTaskModeType.findOriginByLoc(steProtocol.getRow().intValue());
                if (originByLoc.equals(SteTaskModeType.GO_ORIGIN)) {
                    originByLoc = SteTaskModeType.BACK_ORIGIN;
                } else if (originByLoc.equals(SteTaskModeType.BACK_ORIGIN)) {
                    originByLoc = SteTaskModeType.GO_ORIGIN;
                } else {
                    return;
                }
                steCommand.setTaskMode(originByLoc);

                steCommand.setRow(Utils.getGroupRow(steProtocol.getRow().intValue(), false).shortValue());
                steCommand.setBay(steProtocol.getBay());
                steCommand.setLev(steProtocol.getLev());

                if (!MessageQueue.offer(SlaveType.Ste, steNo, new Task(2, steCommand))) {
                    News.error("穿梭车命令下发失败，穿梭车号={}，任务数据={}", steNo, JSON.toJSON(steCommand));
                } else {
                    // 修改工作档状态 35.小车就绪 => 36.小车走行
                    wrkCharge.setWrkSts(36L);
                    wrkCharge.setModiTime(new Date());
                    if (!wrkChargeService.updateById(wrkCharge)) {
                        News.error("修改演示任务{}工作档 35.小车就绪 => 36.小车走行 失败!", wrkCharge.getWrkNo());
                    }
                }
            }
        }

    }
    /**
     * 出入库模式切换
     */