rgv功能完善

pjb

2025-06-28 a609def9badafcb8e8ffa97e8d54962271b1e108

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

@@ -1,386 +1,578 @@
package com.zy.asrs.service.impl;

import com.alibaba.fastjson.JSON;
import com.baomidou.mybatisplus.mapper.EntityWrapper;
import com.baomidou.mybatisplus.mapper.Wrapper;
import com.core.common.Cools;
import com.core.exception.CoolException;
import com.zy.asrs.entity.*;
import com.zy.asrs.mapper.WaitPakinMapper;
import com.zy.asrs.mapper.WrkMastMapper;
import com.zy.asrs.service.BasCrnpService;
import com.zy.asrs.service.LocMastService;
import com.zy.asrs.service.StaDescService;
import com.zy.asrs.service.WrkDetlService;
import com.zy.common.model.StartupDto;
import com.zy.core.Slave;
import com.zy.asrs.entity.BasCircularShuttle;
import com.zy.asrs.entity.BasDevpPosition;
import com.zy.asrs.entity.BasRgv;
import com.zy.asrs.entity.WrkMast;
import com.zy.asrs.service.BasCircularShuttleService;
import com.zy.asrs.service.BasDevpPositionService;
import com.zy.asrs.service.WrkMastService;
import com.zy.asrs.utils.SortTheExecutionOfTheCarUtil;
import com.zy.asrs.utils.TimeCalculatorUtils;
import com.zy.common.service.CommonService;
import com.zy.core.cache.MessageQueue;
import com.zy.core.cache.SlaveConnection;
import com.zy.core.enums.CrnStatusType;
import com.zy.core.enums.RgvModeType;
import com.zy.core.enums.RgvStatusType;
import com.zy.core.enums.RgvTaskModeType;
import com.zy.core.enums.SlaveType;
import com.zy.core.model.DevpSlave;
import com.zy.core.model.RgvSlave;
import com.zy.core.model.Task;
import com.zy.core.model.protocol.CrnProtocol;
import com.zy.core.model.protocol.StaProtocol;
import com.zy.core.model.command.RgvCommand;
import com.zy.core.model.protocol.RgvProtocol;
import com.zy.core.properties.SlaveProperties;
import com.zy.core.thread.BarcodeThread;
import com.zy.core.thread.CrnThread;
import com.zy.core.thread.DevpThread;
import com.zy.core.thread.RgvThread;
import com.zy.system.entity.license.LicenseVerify;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.jdbc.core.JdbcTemplate;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.stereotype.Service;
import org.springframework.transaction.annotation.Transactional;

import java.util.Date;
import java.util.List;
import java.util.stream.Collectors;
import java.util.*;

/**
 * Created by vincent on 2020/8/6
 * 立体仓库WCS系统主流程业务
 * Created by IX on 2024/10/01
 */
@Slf4j
@Service("mainService")
@Transactional
public class MainServiceImpl {

    @Autowired
    private CommonService commonService;
    @Autowired
    private SlaveProperties slaveProperties;
    @Autowired
    private WrkMastMapper wrkMastMapper;
    private BasRgvServiceImpl basRgvService;
    @Autowired
    private WrkDetlService wrkDetlService;
    private BasDevpPositionService basDevpPositionService;
    @Autowired
    private WaitPakinMapper waitPakinMapper;
    private BasCircularShuttleService basCircularShuttleService;
    @Autowired
    private LocMastService locMastService;
    private WrkMastService wrkMastService;
    @Autowired
    private StaDescService staDescService;
    @Autowired
    private BasCrnpService basCrnpService;
    @Autowired
    private JdbcTemplate jdbcTemplate;
    private CommonService commonService;

    /**
     * 入库站，根据条码扫描生成入库工作档，工作状态 2
     */
    @Transactional
    public void generateStoreWrkFile() {
        // 根据输送线plc遍历
        for (DevpSlave devp : slaveProperties.getDevp()) {
            // 遍历入库口
            for (DevpSlave.Sta inSta : devp.getInSta()) {
                // 获取条码
                BarcodeThread barcodeThread = (BarcodeThread) SlaveConnection.get(SlaveType.Barcode, inSta.getBarcode());
                String barcode = barcodeThread.getBarcode();
                // 获取入库站信息
                DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, devp.getId());
                StaProtocol staProtocol = devpThread.getStation().get(inSta.getStaNo());
                // 判断是否满足入库条件
                if (staProtocol.isAutoing() && staProtocol.isLoading() && staProtocol.isInreq1()
                        && !staProtocol.isEmptyMk() && staProtocol.isInreq1() && staProtocol.getWorkNo() ==0
                        && staProtocol.isPakMk() && !Cools.isEmpty(barcode)) {
                    // 判断重复工作档
                    WrkMast wrkMast = wrkMastMapper.selectPakInStep1(inSta.getStaNo(), barcode);
                    if (wrkMast != null) {
                        log.warn("工作档中已存在该站状态为1的数据,工作号-{}", wrkMast.getWrkNo());
                        continue;
                    }
                    // 获取入库通知档
                    List<WaitPakin> waitPakins = waitPakinMapper.selectList(new EntityWrapper<WaitPakin>().eq("barcode", barcode));
                    // 工作号
                    int workNo = commonService.getWorkNo(0);
                    // 检索库位
                    List<String> matNos = waitPakins.stream().map(WaitPakin::getMatnr).distinct().collect(Collectors.toList());
                    StartupDto startupDto = commonService.getLocNo(1, 1, inSta.getStaNo(), matNos);
                    String locNo = startupDto.getLocNo();
                    if (!waitPakins.isEmpty()) {
                        // 插入工作明细档
                        wrkDetlService.createWorkDetail(workNo, waitPakins, barcode);
                    } else {
                        log.warn("无此入库条码数据---{}", barcode);
                        continue;
                    }
    @Value("${constant-parameters.perimeter}")
    private Long perimeter;
    @Value("${constant-parameters.proportion}")
    private Long proportion;
    @Value("${constant-parameters.acceleration}")
    private Long acceleration;
    @Value("${constant-parameters.rgvCount}")
    private Long rgvCount;
    @Value("${constant-parameters.rgvDate}")
    private Double rgvDate;
    @Value("${wms.url}")
    private String wmsUrl;
    @Value("${wms.reportOutPath}")
    private String wmsOutPath;
    @Value("${wms.reportSwitch}")
    private boolean reportSwitch;
    @Value("${wms.reportSwitchAuto}")
    private boolean reportSwitchAuto;

                    // 插入工作主档
                    wrkMast = new WrkMast();
                    wrkMast.setWrkNo(workNo);
                    wrkMast.setIoTime(new Date());
                    wrkMast.setWrkSts(2L); // 工作状态：2.设备上走
                    wrkMast.setIoType(1); // 入出库状态：1.入库
                    wrkMast.setIoPri(10D); // 优先级：10
                    wrkMast.setCrnNo(startupDto.getCrnNo());
                    wrkMast.setSourceStaNo(startupDto.getSourceStaNo());
                    wrkMast.setStaNo(startupDto.getStaNo());
                    wrkMast.setLocNo(startupDto.getLocNo());
                    wrkMast.setBarcode(barcode); // 托盘码
                    wrkMast.setFullPlt("Y"); // 满板：Y
                    wrkMast.setPicking("N"); // 拣料
                    wrkMast.setExitMk("N"); // 退出
                    wrkMast.setEmptyMk("N"); // 空板
                    wrkMast.setLinkMis("N");
//                    wrkMast.setCtnType(sourceStaNo.getCtnType()); // 容器类型
                    // 操作人员数据
                    wrkMast.setAppeTime(new Date());
                    wrkMast.setModiTime(new Date());
                    Integer insert = wrkMastMapper.insert(wrkMast);
                    if (insert == 0) {
                        throw new CoolException("保存工作档失败");
                    }
                    // 更新目标库位状态
                    LocMast locMast = locMastService.selectById(startupDto.getLocNo());
                    locMast.setLocSts("S"); // S.入库预约
                    locMast.setModiTime(new Date());
                    if (!locMastService.updateById(locMast)){
                        throw new CoolException("改变库位状态失败");
                    }

                    // 更新站点信息 且 下发plc命令
                    barcodeThread.setBarcode("");
                    staProtocol.setWorkNo(workNo);
                    staProtocol.setStaNo(startupDto.getStaNo());
                    staProtocol.setPakMk(false);
                    staProtocol.setInreq1(false);
                    boolean result = MessageQueue.offer(SlaveType.Devp, devp.getId(), new Task<>(4, staProtocol));
                    if (!result) {
                        throw new CoolException("更新plc站点信息失败");
                    }

                }


            }
    /*
    * 验证许可证是否有效
    * */
    public synchronized boolean licenseVerify() {
        try{
//            if (true) {
//                return true;
//            }
            return new LicenseVerify().verify();
        } catch (Exception e){
            log.info("许可证验证失败！！异常：{}",e.getMessage());
        }

        return false;
    }


    /**
     * 拣料、并板、盘点再入库
     * 站点任务检测  更新小车位置信息
     */
    public void stnToCrnStnPick(){
        for (DevpSlave devp : slaveProperties.getDevp()) {
            // 遍历拣料入库口
            for (DevpSlave.Sta pickSta : devp.getPickSta()) {
                // 获取拣料入库站信息
                DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, devp.getId());
                StaProtocol staProtocol = devpThread.getStation().get(pickSta.getStaNo());

                if (staProtocol.isAutoing() && staProtocol.isLoading() && staProtocol.isInreq1()
                    && staProtocol.getWorkNo() > 0 && staProtocol.isPakMk()){
                    WrkMast wrkMast = wrkMastMapper.selectPickStep(staProtocol.getWorkNo());
                    if (wrkMast == null) {
                        // 无拣料数据
                        continue;
                    }
                    if ((wrkMast.getIoType() != 103 && wrkMast.getIoType() != 104 && wrkMast.getIoType() != 1070)
                        || Cools.isEmpty(wrkMast.getStaNo()) || Cools.isEmpty(wrkMast.getSourceStaNo()) ) {
                        continue;
                    }
                    // 保存工作明细档历史档
                    if (wrkMastMapper.saveWrkDetlLog(wrkMast.getWrkNo()) == 0) {
                        throw new CoolException("保存工作明细档历史档失败");
                    }
                    // 保存工作主档历史档
                    if (wrkMastMapper.saveWrkMastLog(wrkMast.getWrkNo()) == 0) {
                        throw new CoolException("保存工作主档历史档失败");
                    }
                    // 获取目标站
                    Wrapper<StaDesc> wrapper = new EntityWrapper<StaDesc>()
                            .eq("type_no", wrkMast.getIoType() - 50)
                            .eq("stn_no", wrkMast.getStaNo()) // 作业站点 = 拣料出库的目标站
                            .eq("crn_no", wrkMast.getCrnNo()); // 堆垛机号
                    StaDesc staDesc = staDescService.selectOne(wrapper);
                    if (Cools.isEmpty(staDesc)) {
                        throw new CoolException("入库路径不存在");
                    }
                    // 堆垛机站点(目标站)
                    Integer staNo = staDesc.getCrnStn();
                    // 更新工作档数据状态
                    wrkMast.setIoType(wrkMast.getIoType() - 50); // 入出库类型: 103->53,104->54,107->57
                    wrkMast.setWrkSts(2L); // 工作状态: 2.设备上走
                    wrkMast.setSourceStaNo(wrkMast.getStaNo()); // 源站
                    wrkMast.setStaNo(staNo); // 目标站
                    wrkMast.setLocNo(wrkMast.getSourceLocNo()); // 目标库位 = 出库时的源库位
                    wrkMast.setSourceLocNo(""); // 源库位清空
                    wrkMast.setModiTime(new Date());
                    if (wrkMastMapper.updateById(wrkMast) == 0) {
                        throw new CoolException("更新工作档数据状态失败");
                    }
                    // 修改库位状态 Q.拣料/盘点/并板再入库
                    LocMast locMast = locMastService.selectById(wrkMast.getLocNo());
                    locMast.setLocSts("Q");
                    locMast.setModiTime(new Date());
                    if (!locMastService.updateById(locMast)) {
                        throw new CoolException("修改库位状态失败");
                    }

                    // 更新站点信息 且 下发plc命令
                    staProtocol.setWorkNo(wrkMast.getWrkNo());
                    staProtocol.setStaNo(wrkMast.getStaNo());
                    staProtocol.setPakMk(false);
                    staProtocol.setInreq1(false);
                    boolean result = MessageQueue.offer(SlaveType.Devp, devp.getId(), new Task<>(4, staProtocol));
                    if (!result) {
                        throw new CoolException("更新plc站点信息失败");
                    }

    public synchronized void updateStePositionNearby() {
        try {
            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("wrk_sts", 9L).eq("rgv_no",0));
            if (wrkMasts.isEmpty()) {
                return;
            }
            List<BasDevpPosition> basDevpPositions = basDevpPositionService.selectList(new EntityWrapper<BasDevpPosition>().orderBy("plc_position", true));
            if (basDevpPositions.isEmpty()) {
                log.error("获取所有站点信息异常");
                return;
            }
            List<Integer> siteList = SortTheExecutionOfTheCarUtil.WrkMastExtractSites(wrkMasts);
            List<Integer> devpList = SortTheExecutionOfTheCarUtil.BasDevpPositionExtractSites(basDevpPositions);
            Integer siteNo = SortTheExecutionOfTheCarUtil.ObtainDenseAreasFirstPlace(siteList, devpList);
            if (siteNo == null) {
                log.error("就近密集原则获取站点失败");
                return;
            }
            Long sitePosition = 0L;
            for (BasDevpPosition basDevpPosition : basDevpPositions) {
                if (basDevpPosition.getDevNo().equals(siteNo)) {
                    sitePosition = basDevpPosition.getPlcPosition();
                }

            }
            List<List<Long>> rgvPositionList = new ArrayList<>();
            for (RgvSlave rgvSlave : slaveProperties.getRgv()) {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvSlave.getId());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
                    continue;
                }
                List<Long> rgvPosition = new ArrayList<>();
                rgvPosition.add(rgvProtocol.getRgvNo().longValue());
                rgvPosition.add(rgvProtocol.getRgvPos());
                rgvPositionList.add(rgvPosition);
            }
            Integer rgvNo = SortTheExecutionOfTheCarUtil.LatelyAndGreaterThan(rgvPositionList, sitePosition, perimeter);
            if (rgvNo == -1) {
                log.info("更新小车排序信息异常={}", rgvNo);
                return;
            }
            List<BasCircularShuttle> basCircularShuttleList = basCircularShuttleService.selectList(new EntityWrapper<BasCircularShuttle>().eq("status", 0).orderBy("rgv_id", true));
            if (basCircularShuttleList.get(0).getRgvNo().equals(rgvNo)) {
                return;
            }

            int[][] ints = new int[basCircularShuttleList.size()][2];
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                ints[basCircularShuttle.getRgvNo() - 1] = new int[]{basCircularShuttle.getRgvNo(), basCircularShuttle.getRgvId()};
            }
//            String[] oldList = new String[ints.length];
//            for (int[] rgv : ints){
//                oldList[rgv[0]-1] = Arrays.toString(rgv);
//            }
            int[][] rgvList = SortTheExecutionOfTheCarUtil.ReorderSteId(ints, rgvNo);
//            String[] newList = new String[rgvList.length];
//            for (int[] rgv : rgvList){
//                newList[rgv[0]-1] = Arrays.toString(rgv);
//            }

//            log.info("更新小车排序信息：原始小车号rgvNo={},小车重新排序信息={},小车原始排序信息={}",rgvNo,Arrays.toString(newList),Arrays.toString(oldList));
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                int[] rgv = rgvList[basCircularShuttle.getRgvNo() - 1];
                basCircularShuttle.setRgvId(rgv[1]);
                basCircularShuttleService.updateById(basCircularShuttle);
            }

        } catch (Exception e) {
            log.error("自动更新小车排序信息失败，异常：" + e);
        }
    }

    /**
     * 堆垛机站出库到出库站
     * 站点任务检测  更新小车位置信息
     */
    public void crnStnToOutStn() {
        for (DevpSlave devp : slaveProperties.getDevp()) {
            // 遍历拣料入库口
            for (DevpSlave.Sta outSta : devp.getOutSta()) {
                // 获取堆垛机出库站信息
                DevpThread devpThread = (DevpThread) SlaveConnection.get(SlaveType.Devp, devp.getId());
                StaProtocol staProtocol = devpThread.getStation().get(outSta.getStaNo());
                if (staProtocol.isAutoing() && staProtocol.isLoading() && (staProtocol.getWorkNo() == 0 || staProtocol.getStaNo() == null)) {
                    // 查询工作档
                    WrkMast wrkMast = wrkMastMapper.selectPakOutStep1(staProtocol.getSiteId());
                    if (wrkMast == null) {
                        continue;
                    }
                    // 判断工作档条件
                    if (wrkMast.getIoType() < 100 || wrkMast.getStaNo() == null || wrkMast.getSourceStaNo() == null) {
                        continue;
                    }
                    // 判断吊车是否实际已完成，且电脑状态在move中，以备电脑进行更新工作档
                    CrnThread crnThread = (CrnThread) SlaveConnection.get(SlaveType.Crn, wrkMast.getCrnNo());
                    CrnProtocol crnProtocol = crnThread.getCrnProtocol();
                    if (crnProtocol.statusType == CrnStatusType.FETCHING || crnProtocol.statusType == CrnStatusType.PUTTING) {
                        // 移动中
                        continue;
                    }
                    // 更新工作档状态为14失败 todo:luxiaotao
                    if (crnProtocol.mode == 3 && crnProtocol.getTaskNo() == wrkMast.getWrkNo()
                            && crnProtocol.statusType == CrnStatusType.IDLE
                            && crnProtocol.forkPos==0) {
                        wrkMast.setWrkSts(14L);
                        wrkMast.setCrnEndTime(new Date());
                        if (wrkMastMapper.updateById(wrkMast) == 0) {
                            throw new CoolException("更新工作档的工作状态为14失败，工作号"+wrkMast.getWrkNo());
    public synchronized void updateStePosition() {
        try {
            boolean sign = false;
            Integer rgvNo = 0;
            List<BasCircularShuttle> basCircularShuttleList = basCircularShuttleService.selectList(new EntityWrapper<BasCircularShuttle>().orderBy("rgv_id", true));
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                if (basCircularShuttle.getStatus() != 0){
                    sign = true;
                    continue;
                }
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, basCircularShuttle.getRgvNo());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
                    sign = true;
                } else if (rgvProtocol.modeType != RgvModeType.AUTO || (rgvProtocol.getStatusType() != RgvStatusType.IDLE && rgvProtocol.getStatusType() != RgvStatusType.ROAM)) {
                    sign = true;
                } else {
                    rgvNo = basCircularShuttle.getRgvNo();
                    break;
                }
            }
            if (sign && rgvNo != 0) {
                int[][] ints = new int[basCircularShuttleList.size()][2];
                for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                    ints[basCircularShuttle.getRgvNo() - 1] = new int[]{basCircularShuttle.getRgvNo(), basCircularShuttle.getRgvId()};
                }
//                String[] oldList = new String[ints.length];
//                for (int[] rgv : ints){
//                    oldList[rgv[0]-1] = Arrays.toString(rgv);
//                }
                int[][] rgvList = SortTheExecutionOfTheCarUtil.ReorderSteId(ints, rgvNo);
//                String[] newList = new String[rgvList.length];
//                for (int[] rgv : rgvList){
//                    newList[rgv[0]-1] = Arrays.toString(rgv);
//                }

//                log.info("更新小车排序信息：原始小车号rgvNo={},小车重新排序信息={},小车原始排序信息={}",rgvNo,Arrays.toString(newList),Arrays.toString(oldList));
                for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                    int[] rgv = rgvList[basCircularShuttle.getRgvNo() - 1];
                    basCircularShuttle.setRgvId(rgv[1]);
                    basCircularShuttleService.updateById(basCircularShuttle);
                }
            }

        } catch (Exception e) {
            log.error("自动更新小车排序信息失败，异常：" + e);
        }
    }

    /**
     * 站点任务检测  下发小车取放任务
     */
    public synchronized void DevpTaskNoRun() {
        try {
            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("rgv_no", 0).eq("wrk_sts",9).orderBy("modi_time", true));
            if (wrkMasts.isEmpty()) {
                return;
            }
            long differenceInSeconds = TimeCalculatorUtils.differenceInMilliseconds(wrkMasts.get(0).getModiTime(), new Date());
            if (differenceInSeconds <= 100) {
                return;
            }

            BasCircularShuttle basCircularShuttle = basCircularShuttleService.selectOne(new EntityWrapper<BasCircularShuttle>().eq("rgv_id", 1));
            RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, basCircularShuttle.getRgvNo());
            RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
            if (rgvProtocol == null) {
                return;
            }
            List<BasDevpPosition> basDevpPositions = basDevpPositionService.selectList(new EntityWrapper<BasDevpPosition>().orderBy("plc_position", true));
            Integer devNo = SortTheExecutionOfTheCarUtil.LatelyAndLessThan(basDevpPositions, rgvProtocol.getRgvPos(), perimeter);
            BasDevpPosition[] basDevpPositionsList = SortTheExecutionOfTheCarUtil.devpNoSort(basDevpPositions, devNo);
            BasDevpPosition[] basDevpPositionsListUN = SortTheExecutionOfTheCarUtil.devpNoSortUN(basDevpPositionsList);
            List<List<WrkMast>> wrkMastLists = getWrkMastLists(basDevpPositionsListUN);
            //下发任务
            taskDown(wrkMastLists);
        } catch (Exception e) {
            log.error("自动下发小车取放任务失败，异常：" + e);
        }
    }

    //获取小车取放任务列表失败
    public synchronized List<List<WrkMast>> getWrkMastLists(BasDevpPosition[] basDevpPositionsListUN) {
        List<List<WrkMast>> wrkMastLists = new ArrayList<>();
        try {
            List<WrkMast> wrkMastlistA = new ArrayList<>();
            List<WrkMast> wrkMastlistB = new ArrayList<>();
            for (BasDevpPosition basDevpPosition : basDevpPositionsListUN) {
                List<WrkMast> wrkMastList = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("rgv_ssta_no", basDevpPosition.getDevNo()).eq("rgv_no", 0).orderBy("modi_time", true));
                for (WrkMast wrkMast : wrkMastList) {
                    if (!Cools.isEmpty(wrkMast)) {
                        if (SortTheExecutionOfTheCarUtil.devpNoSortbj(basDevpPositionsListUN, wrkMast.getSourceStaNo(), wrkMast.getStaNo())) {
                            wrkMastlistA.add(wrkMast);
                        } else {
                            wrkMastlistB.add(wrkMast);
                        }
                    }
                }
            }
            wrkMastLists.add(wrkMastlistA);
            wrkMastLists.add(wrkMastlistB);
            return wrkMastLists;
        } catch (Exception e) {
            log.error("获取小车取放任务列表失败，异常：" + e);
            return wrkMastLists;
        }
    }

                    // 命令下发区 --------------------------------------------------------------------------

                    // 1.复位堆垛机 更新堆垛机信息 且 下发plc命令 todo:luxiaotao
                    crnProtocol.setStatus(CrnStatusType.IDLE);
                    crnProtocol.setTaskNo((short)0);
                    if (!MessageQueue.offer(SlaveType.Crn, wrkMast.getCrnNo(), new Task<>(4, crnProtocol))) {
                        throw new CoolException("更新堆垛机信息失败");
    public synchronized void taskDown(List<List<WrkMast>> wrkMastLists) {
        long rgvId = 0;
        runRgv:
        for (List<WrkMast> wrkMastList : wrkMastLists) {
            for (WrkMast wrkMast : wrkMastList) {
                while (rgvId <= rgvCount) {
                    rgvId++;
//                    log.info("存在任务，RGV号={}，任务数据={}", rgvId, JSON.toJSON(wrkMast));
                    if (rgvId > rgvCount) {
                        break runRgv;
                    }
                    BasCircularShuttle basCircularShuttle = basCircularShuttleService.selectOne(new EntityWrapper<BasCircularShuttle>().eq("rgv_id", rgvId));
                    if (basCircularShuttle.getStatus() != 0){
                        continue ;
                    }
                    RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, basCircularShuttle.getRgvNo());
                    RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                    if (rgvProtocol == null) {
                        continue;
                    }

                    // 2.下发站点信息
                    staProtocol.setWorkNo(wrkMast.getWrkNo());
                    staProtocol.setStaNo(wrkMast.getStaNo());
                    if (!MessageQueue.offer(SlaveType.Devp, devp.getId(), new Task<>(4, staProtocol))) {
                        throw new CoolException("更新plc站点信息失败");
                    List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("rgv_no", rgvProtocol.getRgvNo()).eq("wrk_sts",10));
                    if (!wrkMasts.isEmpty()) {
                        continue;
                    }
                    if (rgvProtocol.modeType == RgvModeType.AUTO
                            && (rgvProtocol.getStatusType() == RgvStatusType.IDLE || rgvProtocol.getStatusType() == RgvStatusType.ROAM)
                            && rgvProtocol.getTaskNo1() == 0
                            && rgvProtocol.getAlarm() == 0) {
//                        if (rgvProtocol.getStatusType() == RgvStatusType.ROAM) {
//                            double finalVelocity = 0.0;     // 最终速度 (m/s)
//                            double distance = (Math.pow(finalVelocity, 2) - Math.pow(rgvProtocol.instantaneousSpeed / 60, 2)) / (2 * acceleration);
//                            BasDevpPosition basDevpPosition = basDevpPositionService.selectOne(new EntityWrapper<BasDevpPosition>().eq("dev_no", wrkMast.getSourceStaNo()));
//                            if ((distance * proportion + (rgvProtocol.instantaneousSpeed / 60) * proportion * rgvDate) > (SortTheExecutionOfTheCarUtil.LatelyAndLessThan(basDevpPosition.getPlcPosition(), rgvProtocol.getRgvPos(), perimeter) )) {
//                                List<BasDevpPosition> basDevpPositions = basDevpPositionService.selectList(new EntityWrapper<BasDevpPosition>().orderBy("plc_position", true));
//                                if (basDevpPositions.isEmpty()) {
//                                    log.error("获取所有站点信息异常,RGV任务下发失败，请联系管理员！！！");
//                                    break runRgv;
//                                }
//                                List<Integer> devpList = SortTheExecutionOfTheCarUtil.BasDevpPositionExtractSites(basDevpPositions);
//
//                                if (!SortTheExecutionOfTheCarUtil.calculateShortestDistanceDirection(devpList,rgvProtocol.getEndStaM(),wrkMast.getSourceStaNo())){
//                                    continue;
//                                }
//                            }
//                        }
                        RgvCommand rgvCommand = new RgvCommand();
                        rgvCommand.setRgvNo(rgvProtocol.getRgvNo());
                        rgvCommand.setAckFinish1((short) 0);  // 工位1任务完成确认位
                        rgvCommand.setTaskNo1(wrkMast.getWrkNo().shortValue());
                        rgvCommand.setTaskMode1(RgvTaskModeType.FETCH_PUT); // 工位1任务模式:  取放货
                        rgvCommand.setSourceStaNo1(wrkMast.getRgvSstaNo().shortValue());   //工位1起点
                        rgvCommand.setDestinationStaNo1(wrkMast.getRgvDstaNo().shortValue());   //工位1目标站点
                        rgvCommand.setCommand((short) 1);   //工位1任务确认
                        if (!MessageQueue.offer(SlaveType.Rgv, rgvProtocol.getRgvNo(), new Task(2, rgvCommand))) {
                            //
                            log.error("RGV命令下发失败，RGV号={}，任务数据={}", rgvId, JSON.toJSON(rgvCommand));
                            break runRgv;
                        }
                        log.info("RGV命令下发成功，RGV号={}，任务数据={}", rgvId, JSON.toJSON(rgvCommand));

                        wrkMast.setRgvNo(rgvProtocol.getRgvNo());
                        wrkMast.setWrkSts(10L);
                        wrkMast.setAppeTime(new Date());
                        try {
                            wrkMastService.updateById(wrkMast);
                            break;
                        } catch (Exception e) {
                            log.error("更新小车任务失败,任务号：" + wrkMast.getWrkNo());
                        }
                        break runRgv;
                    } else {
                        break runRgv;
                    }
                }
            }
        }
    }

    /**
     * 入出库  ===>>  堆垛机站到库位 堆垛机入出库作业下发
     */
    public void crnIoExecute(){
        for (Slave crn : slaveProperties.getCrn()) {
            // 获取堆垛机信息
            CrnThread crnThread = (CrnThread) SlaveConnection.get(SlaveType.Crn, crn.getId());
            CrnProtocol crnProtocol = crnThread.getCrnProtocol();
            BasCrnp basCrnp = basCrnpService.selectById(crn.getId());
            if (basCrnp == null) {
                continue;
    public synchronized void rgvTheHandlingTaskIsIssued() {
        try {
            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("rgv_no", 0).eq("wrk_sts", 9)
                    .orderBy("io_pri", false)
                    .orderBy("modi_time", true));
            if (wrkMasts.isEmpty()) {
                return;
            }
            // 只有当堆垛机空闲 或者 无任务时才继续执行
            if (crnProtocol.getStatusType() == CrnStatusType.IDLE || crnProtocol.getTaskNo() == 0) {
                // 如果最近一次是入库模式
                if (crnProtocol.getLastIo().equals("I")) {
                    if (basCrnp.getInEnable().equals("Y")) {
                        this.crnStnToLoc(); //  入库
                        crnProtocol.setLastIo("O");
                    } else if (basCrnp.getOutEnable().equals("Y")) {
                        this.locToCrnStn(); //  出库
                        crnProtocol.setLastIo("I");
                    }

            LinkedList<RgvProtocol> rgvProtocolList = new LinkedList<>();
            for (int i = 1; i <= 5; i++) {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, i);
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
                    continue;
                }
                // 如果最近一次是出库模式
                else if (crnProtocol.getLastIo().equals("O")) {
                    if (basCrnp.getOutEnable().equals("Y")) {
                        this.locToCrnStn(); //  出库
                        crnProtocol.setLastIo("I");
                    } else if (basCrnp.getInEnable().equals("Y")) {
                        this.crnStnToLoc(); //  入库
                        crnProtocol.setLastIo("O");
                rgvProtocolList.add(rgvProtocol);
            }

            runRgv:
            for (WrkMast wrkMast : wrkMasts) {
                Integer rgvSstaNo = wrkMast.getRgvSstaNo();
                // 1090，1089,1086,1083 按1083站点位置计算，能取1083，就能取1090，1089，1086，避免任务延迟下发方案一直给1083发
                if (rgvSstaNo == 1090 || rgvSstaNo == 1089 || rgvSstaNo == 1086) {
                    rgvSstaNo = 1083;
                }
                BasDevpPosition basDevpPositions = basDevpPositionService.selectOne(new EntityWrapper<BasDevpPosition>().eq("dev_no", rgvSstaNo)); // 站点
                long position = basDevpPositions.getPlcPosition(); // 起始站位置
                TreeMap<Long, RgvProtocol> map = new TreeMap<>();
                for (RgvProtocol rgvProtocol : rgvProtocolList) {
                    long distance;
                    if (rgvProtocol.getRgvPos() <= position) {
                        distance = position - rgvProtocol.getRgvPos();
                    } else {
                        distance = position + (perimeter - rgvProtocol.getRgvPos());
                    }
                    map.put(distance, rgvProtocol);
                }
//                log.info("取货站点：{}，后面小车排序", basDevpPositions.getDevNo());
//                StringBuilder sb = new StringBuilder();
//                for (Map.Entry<Long, RgvProtocol> rgvProtocolMap : map.entrySet()) {
//                    RgvProtocol rgvProtocol = rgvProtocolMap.getValue();
//                    sb.append("[").append(rgvProtocol.getRgvNo()).append(",").append(rgvProtocol.getStatusType().desc).append("]");
//                }
//                log.info(sb.toString());

                for (Map.Entry<Long, RgvProtocol> rgvProtocolMap : map.entrySet()) {
                    RgvProtocol rgvProtocol = rgvProtocolMap.getValue(); // 最近距离小车
//                    log.info("取货站点：{}，后面最近小车：{},状态:{}", basDevpPositions.getDevNo(), rgvProtocol.getRgvNo(), rgvProtocol.getStatusType().desc);
                    int count = wrkMastService.selectCount(new EntityWrapper<WrkMast>().eq("rgv_no", rgvProtocol.getRgvNo()).eq("wrk_sts", 10));
                    if(wrkMast.getIoType() == 10 || wrkMast.getIoType() == 110 || wrkMast.getSourceStaNo() == 1034) {
                        if (count != 0) {
                            continue;
                        }
                        if (rgvProtocol.modeType == RgvModeType.AUTO
                                && (rgvProtocol.getStatusType() == RgvStatusType.IDLE || rgvProtocol.getStatusType() == RgvStatusType.ROAM)
                                && rgvProtocol.getTaskNo1() == 0
                                && rgvProtocol.getAlarm() == 0) {
                            RgvCommand rgvCommand = new RgvCommand();
                            rgvCommand.setRgvNo(rgvProtocol.getRgvNo());
                            rgvCommand.setAckFinish1((short) 0);  // 工位1任务完成确认位
                            rgvCommand.setTaskNo1(wrkMast.getWrkNo().shortValue());
                            rgvCommand.setTaskMode1(RgvTaskModeType.FETCH_PUT); // 工位1任务模式:  取放货
                            rgvCommand.setSourceStaNo1(wrkMast.getRgvSstaNo().shortValue());   //工位1起点
                            rgvCommand.setDestinationStaNo1(wrkMast.getRgvDstaNo().shortValue());   //工位1目标站点
                            rgvCommand.setCommand((short) 1);   //工位1任务确认
                            if (!MessageQueue.offer(SlaveType.Rgv, rgvProtocol.getRgvNo(), new Task(2, rgvCommand))) {
                                log.error("RGV命令下发失败，RGV号={}，任务数据={}", rgvProtocol.getRgvNo(), JSON.toJSON(rgvCommand));
                                break runRgv;
                            }
                            log.info("RGV命令下发成功，RGV号={}，任务数据={}", rgvProtocol.getRgvNo(), JSON.toJSON(rgvCommand));
                            Date now = new Date();
                            wrkMast.setRgvNo(rgvProtocol.getRgvNo());
                            wrkMast.setWrkSts(10L);
                            wrkMast.setRgvCreateTime(now);
                            wrkMastService.updateById(wrkMast);
                            break runRgv;
                        }
                    } else {
                        if (count != 0) {
                            break;
                        }
                        if (rgvProtocol.modeType == RgvModeType.AUTO
                                && (rgvProtocol.getStatusType() == RgvStatusType.IDLE || rgvProtocol.getStatusType() == RgvStatusType.ROAM)
                                && rgvProtocol.getTaskNo1() == 0
                                && rgvProtocol.getAlarm() == 0) {
                            RgvCommand rgvCommand = new RgvCommand();
                            rgvCommand.setRgvNo(rgvProtocol.getRgvNo());
                            rgvCommand.setAckFinish1((short) 0);  // 工位1任务完成确认位
                            rgvCommand.setTaskNo1(wrkMast.getWrkNo().shortValue());
                            rgvCommand.setTaskMode1(RgvTaskModeType.FETCH_PUT); // 工位1任务模式:  取放货
                            rgvCommand.setSourceStaNo1(wrkMast.getRgvSstaNo().shortValue());   //工位1起点
                            rgvCommand.setDestinationStaNo1(wrkMast.getRgvDstaNo().shortValue());   //工位1目标站点
                            rgvCommand.setCommand((short) 1);   //工位1任务确认
                            if (!MessageQueue.offer(SlaveType.Rgv, rgvProtocol.getRgvNo(), new Task(2, rgvCommand))) {
                                log.error("RGV命令下发失败，RGV号={}，任务数据={}", rgvProtocol.getRgvNo(), JSON.toJSON(rgvCommand));
                                break runRgv;
                            }
                            log.info("RGV命令下发成功，RGV号={}，任务数据={}", rgvProtocol.getRgvNo(), JSON.toJSON(rgvCommand));
                            Date now = new Date();
                            wrkMast.setRgvNo(rgvProtocol.getRgvNo());
                            wrkMast.setWrkSts(10L);
                            wrkMast.setRgvCreateTime(now);
                            wrkMastService.updateById(wrkMast);
                            break runRgv;
                        } else {
                            break ; // 只看最近的第一个小车
                        }
                    }
                }
            }
            // 库位移转
            this.locToLoc();

        } catch (Exception e) {
            log.error("自动下发小车取放任务失败，异常：" + e);
        }
    }

    /**
     * 入库  ===>>  堆垛机站到库位
     */
    private void crnStnToLoc(){
    //任务完成
    public synchronized void rgvCompleteWrkMastSta(int[] autoZ) {
        try {
            for (RgvSlave rgvSlave : slaveProperties.getRgv()) {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvSlave.getId());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
                    continue;
                }
                BasRgv basRgv = basRgvService.selectById(rgvSlave.getId());
                if (basRgv == null) {
                    log.error("{}号RGV尚未在数据库进行维护！", rgvSlave.getId());
                    continue;
                }

    }

    /**
     * 出库  ===>>  库位到堆垛机站
     */
    private void locToCrnStn(){

    }

    /**
     * 库位移转
     */
    private void locToLoc(){

    }

    /**
     * 执行对工作档的入库完成
     */
    public void storeFinished() {
        for (Slave crn : slaveProperties.getCrn()) {
            // 获取堆垛机信息
            CrnThread crnThread = (CrnThread) SlaveConnection.get(SlaveType.Crn, crn.getId());
            CrnProtocol crnProtocol = crnThread.getCrnProtocol();

        }
    }

    /**
     * 更新堆垛机移动时工作档状态
     */
    public void updateCrnMove() {
        for (Slave crn : slaveProperties.getCrn()) {
            // 获取堆垛机信息
            CrnThread crnThread = (CrnThread) SlaveConnection.get(SlaveType.Crn, crn.getId());
            CrnProtocol crnProtocol = crnThread.getCrnProtocol();
            // todo
            if (crnProtocol.getStatusType() == CrnStatusType.FETCH_POSITION) {

                WrkMast wrkMast = wrkMastMapper.selectById(crnProtocol.getTaskNo());


                // 只有当RGV等待WCS确认、自动
                if (rgvProtocol.getStatusType() == RgvStatusType.WAITING
                        && rgvProtocol.getModeType() == RgvModeType.AUTO
                        && rgvProtocol.getTaskNo1() != 0
                ) {
                    log.info("{}号小车等待wcs确认，状态{}，参数{}",rgvProtocol.getRgvNo(),rgvProtocol.getStatusType(),rgvProtocol);
                    WrkMast wrkMast = wrkMastService.selectOne(new EntityWrapper<WrkMast>().eq("wrk_no", rgvProtocol.getTaskNo1().longValue()));
                    if (Cools.isEmpty(wrkMast)) {
                        log.error("未查到小车执行任务！" + rgvProtocol.getTaskNo1());
                        continue;
                    } else if(wrkMast.getWrkSts() != 10) {
                        log.error("执行任务状态不符合！" + rgvProtocol.getTaskNo1());
                        continue;
                    }
                    boolean rgvComplete = rgvComplete(rgvProtocol.getRgvNo());
                    if (!rgvComplete) {
                            log.error("小车复位失败，小车号{}！", rgvProtocol.getRgvNo());
                        break;
                    }
                    wrkMast.setWrkSts(2L);
                    Date now = new Date();
                    wrkMast.setRgvFinishTime(now);
                    wrkMastService.updateById(wrkMast);
                }
            }
        } catch (Exception e) {
            log.error("小车复位线程报错！" + e);
        }
    }

    // 小车取货越位,任务取消
    public synchronized void rgvTaskCancel() {
        try {
            for (RgvSlave rgvSlave : slaveProperties.getRgv()) {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvSlave.getId());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
                    continue;
                }

                // 只有当RGV等待WCS确认、自动
                if (rgvProtocol.getStatusType() == RgvStatusType.PICK_UP_OFFSIDE
                        && rgvProtocol.getModeType() == RgvModeType.AUTO
                        && rgvProtocol.getTaskNo1() != 0
                ) {
                    log.info("{}号小车小车取货越位，状态{}，参数{}",rgvProtocol.getRgvNo(),rgvProtocol.getStatusType(),rgvProtocol);
                    WrkMast wrkMast = wrkMastService.selectOne(new EntityWrapper<WrkMast>().eq("wrk_no", rgvProtocol.getTaskNo1().longValue()));
                    if (Cools.isEmpty(wrkMast)) {
                        log.error("未查到小车执行任务！" + rgvProtocol.getTaskNo1());
                        continue;
                    } else if(wrkMast.getWrkSts() != 10) {
                        log.error("执行任务状态不符合！" + rgvProtocol.getTaskNo1());
                        continue;
                    }
                    boolean rgvComplete = rgvComplete(rgvProtocol.getRgvNo());
                    if (!rgvComplete) {
                        log.error("小车取货越位,任务取消,复位失败，小车号{}！", rgvProtocol.getRgvNo());
                        break;
                    }
                    wrkMast.setWrkSts(9L);
                    wrkMast.setRgvNo(0);
                    wrkMast.setRgvCreateTime(new Date());
                    wrkMastService.updateById(wrkMast);
                }
            }
        } catch (Exception e) {
            log.error("小车复位线程报错！" + e);
        }
    }

    /*
     * 小车复位
     * */
    public synchronized boolean rgvComplete(Integer rgvNo) {
        try {
            //  命令下发区 --------------------------------------------------------------------------
            if (!MessageQueue.offer(SlaveType.Rgv, rgvNo, new Task(3, new RgvCommand()))) {
                log.error("小车复位RGV命令下发失败，RGV号={}", rgvNo);
                return false;
            } else {
                log.info("小车复位RGV命令下发成功，RGV号={}", rgvNo);
                return true;
            }
        } catch (Exception e) {
            log.error("RGV命令下发失败，RGV号={}。异常：" + e, rgvNo);
            return false;
        }
    }
}