#输送线读取

Junjie

2023-12-08 ff2b6e2329ae2c5d3fede2f9413170fd6e87c46c

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

@@ -10,7 +10,7 @@

import com.zy.asrs.utils.Utils;
import com.zy.common.model.NavigateNode;
import com.zy.common.model.NyShuttleOperaResult;
import com.zy.common.service.CommonService;
import com.zy.common.utils.*;
import com.zy.core.News;
import com.zy.core.ThreadHandler;
@@ -33,6 +33,7 @@
import java.text.MessageFormat;
import java.util.ArrayList;
import java.util.Date;
import java.util.HashMap;
import java.util.List;

/**
@@ -99,6 +100,11 @@
                //执行下一步指令
                executeWork(shuttleProtocol.getTaskNo().shortValue());
            }

            //小车空闲且有跑库程序
            if (shuttleProtocol.isIdle() && shuttleProtocol.getMoveLoc()) {
                moveLoc();
            }
        } catch (Exception e) {
            e.printStackTrace();
            OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】四向穿梭车plc状态信息失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort()));
@@ -107,12 +113,6 @@

    private void readStatus() {
        try {
            if (null == shuttleProtocol) {
                shuttleProtocol = new NyShuttleProtocol();
                shuttleProtocol.setShuttleNo(slave.getId().shortValue());
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
            }

            //----------读取四向穿梭车状态-----------
            NyShuttleHttpCommand readStatusCommand = NyHttpUtils.getReadStatusCommand(slave.getId());
            JSONObject jsonObject = NyHttpUtils.requestCommand(socket, readStatusCommand);
@@ -241,6 +241,7 @@
                    OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】[id:{1}] <<<<< 实时数据更新成功",DateUtils.convert(new Date()), slave.getId()));
//                    log.info(MessageFormat.format("【{0}】[id:{1}] <<<<< 实时数据更新成功",DateUtils.convert(new Date()), slave.getId()));
                }
//                log.warn(JSON.toJSONString(shuttleProtocol));//输出小车状态
            }
        } catch (Exception e) {
            e.printStackTrace();
@@ -263,6 +264,11 @@
            socket.setSoTimeout(60000);
            socket.setKeepAlive(true);
            this.socket = socket;
            if (null == shuttleProtocol) {
                shuttleProtocol = new NyShuttleProtocol();
                shuttleProtocol.setShuttleNo(slave.getId().shortValue());
            }
            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
            log.info(MessageFormat.format("【{0}】四向穿梭车Socket链接成功 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort()));
        } catch (IOException e) {
            OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】四向穿梭车Socket链接失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort()));
@@ -329,6 +335,7 @@
            );
            opt.setSend(1);//已下发
            opt.setResponse(JSON.toJSONString(result));//请求响应
            opt.setDeviceWrk(command.getWrkNo().toString());//设备工作号
            shuttleOptService.insert(opt);
        }

@@ -349,7 +356,7 @@
        shuttleProtocol.setAssignCommand(assignCommand);
        shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
        //任务数据保存到redis
        redisUtil.set("shuttle_wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(redisCommand));
        redisUtil.set(RedisKeyType.SHUTTLE.key + assignCommand.getTaskNo(), JSON.toJSONString(redisCommand));
        //执行下发任务
        executeWork(assignCommand.getTaskNo());
    }
@@ -361,9 +368,11 @@
            return false;
        }

        NavigateMapUtils navigateMapUtils = SpringUtils.getBean(NavigateMapUtils.class);
        WrkMastMapper wrkMastMapper = SpringUtils.getBean(WrkMastMapper.class);
        WrkMast wrkMast = wrkMastMapper.selectByWorkNo(wrkNo.intValue());

        Object o = redisUtil.get("shuttle_wrk_no_" + wrkNo);
        Object o = redisUtil.get(RedisKeyType.SHUTTLE.key + wrkNo);
        if (o == null) {
            return false;
        }
@@ -377,8 +386,9 @@
            return false;
        }

        NavigateMapData navigateMapData = new NavigateMapData(Utils.getLev(shuttleProtocol.getCurrentLocNo()));
        checkIOSta(commands, commandStep);//检测小车是否进出提升机输送站

        boolean isLock = false;//是否解锁路径
        //取出命令
        NyShuttleHttpCommand command = null;
        if (commandStep < commands.size()) {
@@ -394,20 +404,30 @@
                if (shuttleProtocol.getPoint().equals(target)) {
                    //上一条指令的目标位置和当前小车位置相同，则认定上一条任务完成
                    lastCommand.setComplete(true);
                    //解锁锁定路径，上一条路径和当前路径
                    List<NavigateNode> nodes = lastCommand.getNodes();
                    if (command != null && command.getNodes() != null) {
                        nodes.addAll(command.getNodes());
                    }
                    //解锁锁定路径，上一条路径
                    List<NavigateNode> nodes = JSON.parseArray(JSON.toJSONString(lastCommand.getNodes()), NavigateNode.class);//进行深度copy
//                    //解锁当前路径
//                    if (command != null && command.getNodes() != null) {
//                        nodes.addAll(command.getNodes());
//                    }
                    if (nodes != null) {
                        navigateMapData.writeNavigateNodeToRedisMap(nodes, false);//解锁路径
                        NavigateNode targetNode = assignCommand.getNodes().get(assignCommand.getNodes().size() - 1);//最终节点
                        NavigateNode node = nodes.get(nodes.size() - 1);
                        if (!(targetNode.getX() == node.getX() && targetNode.getY() == node.getY())) {
                            nodes.remove(nodes.size() - 1);//剔除尾节点
                        }
                        boolean result = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(shuttleProtocol.getCurrentLocNo()), nodes, false);//解锁路径
                        if (!result) {
                            return false;//解锁失败
                        }
                        isLock = true;//解锁过路径
                    }
                }
            }else {
                lastCommand.setComplete(true);//其他命令默认认为完成
            }
            //任务数据保存到redis
            redisUtil.set("shuttle_wrk_no_" + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));
            redisUtil.set(RedisKeyType.SHUTTLE.key + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));

            if (!lastCommand.getComplete()) {
                //上一条任务未完成，禁止下发命令
@@ -418,7 +438,7 @@
            NyShuttleHttpCommand endCommand = commands.get(commands.size() - 1);
            if (endCommand.getComplete()) {
                //删除redis
                redisUtil.del("shuttle_wrk_no_" + redisCommand.getWrkNo());
                redisUtil.del(RedisKeyType.SHUTTLE.key + redisCommand.getWrkNo());

                if (!assignCommand.getCharge()) {
                    //对主线程抛出等待确认状态waiting
@@ -439,8 +459,9 @@
        }

        if (shuttleProtocol.getFree() == ShuttleStatusType.BUSY.id) {
            //停止充电
            if(!(command.getRequest().getBody().get("requestType").equals("stopCharge") && shuttleProtocol.getChargState() == 1)){
            String requestType = command.getRequest().getBody().get("requestType").toString();
            //停止充电 管制命令
            if(!(requestType.equals("stopCharge") && shuttleProtocol.getChargState() == 1) && !requestType.equals("resume")){
                return false;//小车状态忙，禁止执行命令
            }
        }
@@ -455,21 +476,41 @@
            return false;
        }

        if (command.getRequest().getBody().get("requestType").equals("move")) {
            //检测路径是否可行走
            if (!checkPath(command.getNodes(), nextNodes, redisCommand)) {
                return false;
            }

            //锁定路径，锁定当前路径和下一步路径
            List<NavigateNode> nodes = command.getNodes();
            if (nextNodes != null) {
                nodes.addAll(nextNodes);
            }
            if (nodes != null) {
                navigateMapData.writeNavigateNodeToRedisMap(nodes, true);//所使用的路径进行锁定禁用
            }
        }
//        if (command.getRequest().getBody().get("requestType").equals("move")) {
//            ArrayList<int[]> whiteList = new ArrayList<>();//设置节点的白名单
//            if (wrkMast != null && ((wrkMast.getIoType() > 100 && wrkMast.getIoType() < 200) || wrkMast.getIoType() == 11)) {
//                //出库任务，不检测首节点
//                int[] startArr = NavigatePositionConvert.positionToXY(wrkMast.getSourceLocNo());//开始节点
//                whiteList.add(startArr);
//            }
//
//            //解锁过路径，只检测下一段路径是否可走(当前路径已经被锁定无需再检测)
//            if (isLock) {
//                //只检测下一段路径是否可走(当前路径已经被锁定无需再检测)
//                //检测路径是否可行走
//                if (!checkPath(nextNodes == null ? command.getNodes() : nextNodes, null, whiteList)) {
//                    return false;
//                }
//            }else {
//                //检测当前路径和下一段路径
//                //检测路径是否可行走
//                if (!checkPath(command.getNodes(), nextNodes, whiteList)) {
//                    return false;
//                }
//            }
//
//            //锁定路径，锁定当前路径和下一步路径
//            List<NavigateNode> nodes = command.getNodes();
//            if (nextNodes != null) {
//                nodes.addAll(nextNodes);
//            }
//            if (nodes != null) {
//                boolean result = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(shuttleProtocol.getCurrentLocNo()), nodes, true);//所使用的路径进行锁定禁用
//                if (!result) {
//                    return false;//路径锁定失败
//                }
//            }
//        }

        //可执行命令
        if (!write(command, assignCommand)) {
@@ -491,38 +532,7 @@
        //更新redis数据
        redisCommand.setCommandStep(commandStep);
        //任务数据保存到redis
        redisUtil.set("shuttle_wrk_no_" + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));

//        //判断数据是否执行完成
//        if (commandStep < commands.size() - 1) {
//            commandStep++;
//            //更新redis数据
//            redisCommand.setCommandStep(commandStep);
//            //任务数据保存到redis
//            redisUtil.set("shuttle_wrk_no_" + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));
//        }else {
//            //已执行完成
//
//            commandStep = commands.size();
//            //更新redis数据
//            redisCommand.setCommandStep(commandStep);
//            //任务数据保存到redis
//            redisUtil.set("shuttle_wrk_no_" + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));
////            //最后一段命令为移动命令，则暂缓删除redis等待清除路径时一次性删除
////            //最后一段命令为不是移动命令，则删除redis
////            if (!command.getRequest().getBody().get("requestType").equals("move")) {
////                //删除redis
////                redisUtil.del("shuttle_wrk_no_" + redisCommand.getWrkNo());
////            }
////
////            if (!assignCommand.getCharge()) {
////                //对主线程抛出等待确认状态waiting
////                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WAITING);
////            }else {
////                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING);
////            }
////            News.info("四向穿梭车任务执行下发完成等待执行结束，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
//        }
        redisUtil.set(RedisKeyType.SHUTTLE.key + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));

        return true;
    }
@@ -534,7 +544,7 @@
            return false;
        }

        Object o = redisUtil.get("shuttle_wrk_no_" + wrkNo);
        Object o = redisUtil.get(RedisKeyType.SHUTTLE.key + wrkNo);
        if (o == null) {
            return false;
        }
@@ -618,7 +628,7 @@
            return false;
        }

        Object o = redisUtil.get("shuttle_wrk_no_" + wrkNo);
        Object o = redisUtil.get(RedisKeyType.SHUTTLE.key + wrkNo);
        if (o == null) {
            return false;
        }
@@ -658,17 +668,17 @@
     * 检测路径是否可行走
     * 如果路径为目标库位，但不可行走，系统将尝试重新计算路径
     */
    private boolean checkPath(List<NavigateNode> currentNodes, List<NavigateNode> nextNodes, ShuttleRedisCommand redisCommand) {
    private boolean checkPath(List<NavigateNode> currentNodes, List<NavigateNode> nextNodes, List<int[]> whitePoints) {
        //检测路径是否可行走(检查路径锁定状态，检测路径是否有其他小车)
        //检测当前行走路径，和下一步路径
        boolean checkPathIsAvailable = NavigateUtils.checkPathIsAvailable(currentNodes, shuttleProtocol.getShuttleNo().intValue(), Utils.getLev(shuttleProtocol.getCurrentLocNo()));
        boolean checkPathIsAvailable = NavigateUtils.checkPathIsAvailable(currentNodes, shuttleProtocol.getShuttleNo().intValue(), Utils.getLev(shuttleProtocol.getCurrentLocNo()), whitePoints);
        if (nextNodes == null) {
            if (checkPathIsAvailable) {
                return true;//可行走
            }
            return false;
        }else {
            boolean checkPathIsAvailable2 = NavigateUtils.checkPathIsAvailable(nextNodes, shuttleProtocol.getShuttleNo().intValue(), Utils.getLev(shuttleProtocol.getCurrentLocNo()));
            boolean checkPathIsAvailable2 = NavigateUtils.checkPathIsAvailable(nextNodes, shuttleProtocol.getShuttleNo().intValue(), Utils.getLev(shuttleProtocol.getCurrentLocNo()), whitePoints);
            if (checkPathIsAvailable && checkPathIsAvailable2) {
                return true;//可行走
            }
@@ -704,11 +714,164 @@
//            assignCommand.setCommands(commands);
//            redisCommand.setAssignCommand(assignCommand);
//            //任务数据保存到redis
//            redisUtil.set("shuttle_wrk_no_" + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));
//            redisUtil.set(RedisKeyType.SHUTTLE.key + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));
//            return false;//当前不可行走，等待下一次执行走新路径
//        }

        return false;//不可行走
    }

    /**
     * 跑库程序
     */
    private void moveLoc() {
        LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
        ShuttleDispatchUtils shuttleDispatchUtils = SpringUtils.getBean(ShuttleDispatchUtils.class);
        CommonService commonService = SpringUtils.getBean(CommonService.class);
        WrkMastMapper wrkMastMapper = SpringUtils.getBean(WrkMastMapper.class);
        int lev = Utils.getLev(shuttleProtocol.getCurrentLocNo());//小车当前楼层
        if (!shuttleProtocol.isIdle()) {
            return;
        }

        WrkMast wrkMast = wrkMastMapper.selectShuttleHasMoveWorking(shuttleProtocol.getShuttleNo().intValue());
        if (wrkMast != null) {
            return;
        }

        if (shuttleProtocol.getYCurrent() > shuttleProtocol.getYTarget()) {
            //跑库结束
            shuttleProtocol.setMoveLoc(false);
            shuttleProtocol.setMoveType(0);
            shuttleProtocol.setXStart(0);
            shuttleProtocol.setXTarget(0);
            shuttleProtocol.setXCurrent(0);
            shuttleProtocol.setYStart(0);
            shuttleProtocol.setYTarget(0);
            shuttleProtocol.setYCurrent(0);
            return;
        }

        if (shuttleProtocol.getMoveType() == 0) {//跑轨道
            ArrayList<String> locs = new ArrayList<>();
            for (int i = shuttleProtocol.getXCurrent(); i <= shuttleProtocol.getXTarget(); i++) {
                String locNo = Utils.getLocNo(i, shuttleProtocol.getYCurrent(), lev);
                locs.add(locNo);
            }
            List<LocMast> locMasts = locMastService.selectEmptyLocNos(locs);
            if (locMasts.isEmpty()) {
                //空库位
                shuttleProtocol.setYCurrent(shuttleProtocol.getYCurrent() + 1);
                return;
            }

            LocMast start = locMasts.get(0);
            LocMast target = locMasts.get(locMasts.size() - 1);
            //判断小车是否在起点位置
            if (!shuttleProtocol.getCurrentLocNo().equals(start.getLocNo())) {//不在起点位置，调度去起点位置
                shuttleDispatchUtils.dispatchShuttle(commonService.getWorkNo(3), start.getLocNo());
            }else {
                //在起点位置，调度去目标位置
                if (shuttleProtocol.getCurrentLocNo().equals(target.getLocNo())) {
                    shuttleProtocol.setYCurrent(shuttleProtocol.getYCurrent() + 1);//小车和目标位置一致，跳过
                }else {
                    boolean result = shuttleDispatchUtils.dispatchShuttle(commonService.getWorkNo(3), target.getLocNo());
                    if (result) {//调度成功
                        shuttleProtocol.setYCurrent(shuttleProtocol.getYCurrent() + 1);
                    }
                }
            }
        }else {//跑库位
            Integer xCurrent = shuttleProtocol.getXCurrent();
            if (xCurrent > shuttleProtocol.getXTarget()) {//当X值大于X目标值，进行归零且Y方向+1
                shuttleProtocol.setXCurrent(shuttleProtocol.getXStart());
                shuttleProtocol.setYCurrent(shuttleProtocol.getYCurrent() + 1);
                return;
            }

            Integer yCurrent = shuttleProtocol.getYCurrent();
            String locNo = Utils.getLocNo(xCurrent, yCurrent, lev);
            LocMast target = locMastService.selectById(locNo);
            if (target == null) {
                shuttleProtocol.setXCurrent(shuttleProtocol.getXCurrent() + 1);
                return;
            }

            if (!target.getLocSts().equals("O")) {
                shuttleProtocol.setXCurrent(shuttleProtocol.getXCurrent() + 1);
                return;
            }

            //调度去目标位置
            if (shuttleProtocol.getCurrentLocNo().equals(target.getLocNo())) {
                shuttleProtocol.setXCurrent(shuttleProtocol.getXCurrent() + 1);//小车和目标位置一致，跳过
            }else {
                boolean result = shuttleDispatchUtils.dispatchShuttle(commonService.getWorkNo(3), target.getLocNo());
                if (result) {//调度成功
                    shuttleProtocol.setXCurrent(shuttleProtocol.getXCurrent() + 1);
                }
            }
        }
    }

    //检测小车是否进出提升机输送站
    public void checkIOSta(List<NyShuttleHttpCommand> commands, int commandStep) {
        if (commandStep != 0) {
            NyShuttleHttpCommand lastCommand = commands.get(commandStep - 1);//上一步命令
            if (lastCommand.getRequest().getBody().get("requestType").equals("move")) {
                //检测起点是否为提升机输送站点
                NyShuttleProtocol.NyShuttlePointClass start = JSON.parseObject(lastCommand.getRequest().getBody().get("start").toString(), NyShuttleProtocol.NyShuttlePointClass.class);
                int[] startPoint = NavigatePositionConvert.NyXyzToWCSXyz(start.getX(), start.getY(), start.getZ());
                if (startPoint[0] == 13 && (startPoint[1] == 22 || startPoint[1] == 38 || startPoint[1] == 57)) {
                    //输送站点位置

                    int liftNo;
                    if (startPoint[1] == 22) {
                        liftNo = 1;
                    } else if (startPoint[1] == 38) {
                        liftNo = 2;
                    } else {
                        liftNo = 3;
                    }

                    HashMap<String, Object> data = new HashMap<>();
                    data.put("lev", startPoint[2]);
                    data.put("status", false);//出输送站

                    //下发任务
                    MessageQueue.offer(SlaveType.Lift, liftNo, new Task(4, data));
                }
            }
        }

        if (commands.size() == commandStep) {
            return;
        }
        NyShuttleHttpCommand command = commands.get(commandStep);//当前命令
        if (command.getRequest().getBody().get("requestType").equals("move")) {
            NyShuttleProtocol.NyShuttlePointClass target = JSON.parseObject(command.getRequest().getBody().get("target").toString(), NyShuttleProtocol.NyShuttlePointClass.class);
            int[] targetPoint = NavigatePositionConvert.NyXyzToWCSXyz(target.getX(), target.getY(), target.getZ());
            //检测目标位置是否为提升机输送站点
            if (targetPoint[0] == 13 && (targetPoint[1] == 22 || targetPoint[1] == 38 || targetPoint[1] == 57)) {
                //输送站点位置

                int liftNo;
                if (targetPoint[1] == 22) {
                    liftNo = 1;
                } else if (targetPoint[1] == 38) {
                    liftNo = 2;
                } else {
                    liftNo = 3;
                }

                HashMap<String, Object> data = new HashMap<>();
                data.put("lev", targetPoint[2]);
                data.put("status", true);//进输送站

                //下发任务
                MessageQueue.offer(SlaveType.Lift, liftNo, new Task(4, data));
            }
        }
    }

}