*

L

1 天以前 daa2cec25875276f3462e09d102f9d2fd52a96e1

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

@@ -14,6 +14,7 @@
import com.zy.asrs.task.escalationParam.RgvTaskReportingParam;
import com.zy.asrs.utils.PostMesDataUtils;
import com.zy.asrs.utils.SortTheExecutionOfTheCarUtil;
import com.zy.asrs.utils.TaskDispatchPriorityUtil;
import com.zy.asrs.utils.TimeCalculatorUtils;
import com.zy.common.CodeRes;
import com.zy.common.service.CommonService;
@@ -58,6 +59,8 @@
    @Autowired
    private CommonService commonService;

    @Value("${constant-parameters.taskRunPerimeter}")
    private Long taskRunPerimeter;
    @Value("${constant-parameters.perimeter}")
    private Long perimeter;
    @Value("${constant-parameters.proportion}")
@@ -76,6 +79,18 @@
    private boolean reportSwitch;
    @Value("${wms.reportSwitchAuto}")
    private boolean reportSwitchAuto;
    @Value("${constant-parameters.priority.distance-threshold:10000}")
    private Long priorityDistanceThreshold;
    @Value("${constant-parameters.priority.weight.time:0.4}")
    private Double priorityTimeWeight;
    @Value("${constant-parameters.priority.weight.distance:0.4}")
    private Double priorityDistanceWeight;
    @Value("${constant-parameters.priority.weight.region:0.2}")
    private Double priorityRegionWeight;
    @Value("${constant-parameters.priority.region-values:}")
    private String regionPriorityValues;
    private volatile String cachedRegionPriorityRaw = "";
    private volatile Map<Integer, Double> cachedRegionPriorityMap = new HashMap<>();

    /*
    * 验证许可证是否有效
@@ -92,27 +107,15 @@
        return false;
    }


    /**
     * 站点任务检测  更新小车位置信息
     */
    public synchronized void updateStePositionNearby() {
    public synchronized int[][] getStePositionNearby(Integer siteNo) {
        try {
            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("wrk_sts", 1L));
            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;
                return null;
            }
            Long sitePosition = 0L;
            for (BasDevpPosition basDevpPosition : basDevpPositions) {
@@ -136,37 +139,29 @@
            Integer rgvNo = SortTheExecutionOfTheCarUtil.LatelyAndGreaterThan(rgvPositionList, sitePosition, perimeter);
            if (rgvNo == -1) {
                log.info("更新小车排序信息异常={}", rgvNo);
                return;
                return null;
            }
            List<BasCircularShuttle> basCircularShuttleList = basCircularShuttleService.selectList(new EntityWrapper<BasCircularShuttle>().eq("status", 0).orderBy("rgv_id", true));
            if (basCircularShuttleList.get(0).getRgvNo().equals(rgvNo)) {
                return;
            }

            List<BasCircularShuttle> basCircularShuttleList = basCircularShuttleService.selectList(new EntityWrapper<BasCircularShuttle>().orderBy("rgv_id", true));
            int[][] ints = new int[basCircularShuttleList.size()][2];
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                ints[basCircularShuttle.getRgvNo() - 1] = new int[]{basCircularShuttle.getRgvNo(), basCircularShuttle.getRgvId()};
            }
            if (basCircularShuttleList.get(0).getRgvNo().equals(rgvNo)) {
                return ints;
            }
//            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);
//            }
            return rgvList;

//            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);
        }
        return null;

    }

    /**
@@ -226,31 +221,357 @@
     */
    public synchronized void DevpTaskNoRun() {
        try {
            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("wrk_sts", 1L).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);
            dispatchTaskByFlow();
        } catch (Exception e) {
            log.error("自动下发小车取放任务失败，异常：" + e);
            log.error("自动下发小车任务失败，异常：{}", e.getMessage(), e);
        }
    }

    /**
     * 流程图版: 任务池挑任务并下发RGV
     * 时间/距离/区域按权重计算最终优先级，再执行下发。
     */
    private void dispatchTaskByFlow() {
        Date now = new Date();
        List<WrkMast> taskPool = wrkMastService.selectList(
                new EntityWrapper<WrkMast>().eq("wrk_sts", 1L).orderBy("modi_time", true)
        );
        if (taskPool.isEmpty()) {
            return;
        }

        Date firstTaskTime = taskPool.get(0).getAppeTime() == null ? taskPool.get(0).getModiTime() : taskPool.get(0).getAppeTime();
        if (firstTaskTime == null) {
            return;
        }
        long oldestWaitMs = TimeCalculatorUtils.differenceInMilliseconds(firstTaskTime, now);
        if (oldestWaitMs <= 1000) {
            return;
        }

        List<BasDevpPosition> sitePositions = basDevpPositionService.selectList(
                new EntityWrapper<BasDevpPosition>().orderBy("plc_position", true)
        );
        if (sitePositions == null || sitePositions.isEmpty()) {
            return;
        }

        Map<Integer, BasDevpPosition> siteMap = new HashMap<>();
        for (BasDevpPosition site : sitePositions) {
            siteMap.put(site.getDevNo(), site);
        }

        List<RgvRuntimeState> availableRgvs = loadAvailableRgvs();
        if (availableRgvs.isEmpty()) {
            return;
        }
        Map<Integer, RgvRuntimeState> rgvStateMap = new HashMap<>();
        List<TaskDispatchPriorityUtil.RgvSnapshot> snapshots = new ArrayList<>();
        for (RgvRuntimeState rgv : availableRgvs) {
            if (rgv.protocol == null || rgv.protocol.getRgvPos() == null) {
                continue;
            }
            rgvStateMap.put(rgv.shuttle.getRgvNo(), rgv);
            snapshots.add(new TaskDispatchPriorityUtil.RgvSnapshot(rgv.shuttle.getRgvNo(), rgv.protocol.getRgvPos()));
        }
        if (snapshots.isEmpty()) {
            return;
        }

        double[] weights = resolveWeights(taskPool.size(), snapshots.size());
        List<TaskDispatchPriorityUtil.DispatchPlan> plans = TaskDispatchPriorityUtil.rankDispatchPlans(
                taskPool,
                siteMap,
                snapshots,
                parseRegionPriorityMap(),
                now,
                Math.max(1L, priorityDistanceThreshold == null ? 10000L : priorityDistanceThreshold),
                Math.max(1L, perimeter == null ? 1L : perimeter),
                weights[0],
                weights[1],
                weights[2]
        );
        if (plans.isEmpty()) {
            return;
        }

        TaskDispatchPriorityUtil.DispatchPlan topPlan = plans.get(0);
        WrkMast task = topPlan.getTask();
        if (task == null || task.getSourceStaNo() == null) {
            return;
        }
        BasDevpPosition source = siteMap.get(task.getSourceStaNo());
        if (source == null) {
            return;
        }
        RgvRuntimeState chosenRgv = rgvStateMap.get(topPlan.getRgvNo());
        if (chosenRgv == null) {
            return;
        }

        log.info("任务优先级计算: wrkNo={} rgvNo={} final={} (time={} distance={} region={}) distance={} inRange={}",
                task.getWrkNo(),
                topPlan.getRgvNo(),
                topPlan.getFinalPriority(),
                topPlan.getTimePriority(),
                topPlan.getDistancePriority(),
                topPlan.getRegionPriority(),
                topPlan.getDistance(),
                topPlan.isInRange());

        if (topPlan.isInRange() && canFetchFromRoam(chosenRgv.protocol, source.getPlcPosition())) {
            if (issueFetchTask(task, chosenRgv)) {
                updateStePosition();
            }
            return;
        }
        issueRoamTask(chosenRgv, source.getPlcPosition());
    }

    private List<RgvRuntimeState> loadAvailableRgvs() {
        List<RgvRuntimeState> result = new ArrayList<>();
        List<BasCircularShuttle> shuttles = basCircularShuttleService.selectList(
                new EntityWrapper<BasCircularShuttle>().orderBy("rgv_id", true)
        );
        if (shuttles == null || shuttles.isEmpty()) {
            return result;
        }

        for (BasCircularShuttle shuttle : shuttles) {
            try {
                if (shuttle.getStatus() != 0) {
                    continue;
                }
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, shuttle.getRgvNo());
                if (rgvThread == null) {
                    continue;
                }
                RgvProtocol protocol = rgvThread.getRgvProtocol();
                if (!isAvailableRgv(protocol)) {
                    continue;
                }
                if (isRgvRunningTask(shuttle.getRgvNo())) {
                    continue;
                }
                result.add(new RgvRuntimeState(shuttle, protocol));
            } catch (Exception ignore) {
            }
        }
        return result;
    }

    private boolean isAvailableRgv(RgvProtocol protocol) {
        return protocol != null
                && protocol.getModeType() == RgvModeType.AUTO
                && (protocol.getStatusType() == RgvStatusType.IDLE || protocol.getStatusType() == RgvStatusType.ROAM)
                && protocol.getTaskNo1() == 0
                && protocol.getRgvPosInt() != 0
                && protocol.getAlarm() == 0;
    }

    private boolean isRgvRunningTask(Integer rgvNo) {
        List<WrkMast> running = wrkMastService.selectList(
                new EntityWrapper<WrkMast>().eq("rgv_no", rgvNo).eq("wrk_sts", 2L)
        );
        return running != null && !running.isEmpty();
    }

    private boolean canFetchFromRoam(RgvProtocol protocol, long sourcePos) {
        if (protocol.getStatusType() != RgvStatusType.ROAM) {
            return true;
        }
        if (protocol.getInstantaneousSpeed() == null || acceleration == null || acceleration == 0) {
            return true;
        }
        double finalVelocity = 0.0;
        double distance = (Math.pow(finalVelocity, 2) - Math.pow(protocol.getInstantaneousSpeed() / 60, 2)) / (2 * acceleration);
        double threshold = distance * proportion + (protocol.getInstantaneousSpeed() / 60) * proportion * rgvDate;
        double sourceDistance = SortTheExecutionOfTheCarUtil.LatelyAndLessThan(sourcePos, protocol.getRgvPos(), perimeter);
        return !(threshold > sourceDistance);
    }

    private boolean issueFetchTask(WrkMast task, RgvRuntimeState rgv) {
        RgvCommand command = new RgvCommand();
        command.setRgvNo(rgv.shuttle.getRgvNo());
        command.setAckFinish1((short) 0);
        command.setTaskNo1(task.getWrkNo().shortValue());
        command.setTaskMode1(RgvTaskModeType.FETCH_PUT);
        command.setSourceStaNo1(task.getSourceStaNo().shortValue());
        command.setDestinationStaNo1(task.getStaNo().shortValue());
        command.setCommand((short) 1);
        command.setRgvSome(rgv.shuttle.getRgvSome() == 1 ? (short) 1 : (short) 0);

        if (!MessageQueue.offer(SlaveType.Rgv, rgv.shuttle.getRgvNo(), new Task(2, command))) {
            task.setLogErrTime(new Date());
            task.setLogErrMemo("RGV命令下发失败，RGV号={" + rgv.shuttle.getRgvNo() + "}===>跳过");
            wrkMastService.updateById(task);
            log.error("RGV命令下发失败，RGV号={}，任务数据={}", rgv.shuttle.getRgvNo(), JSON.toJSON(command));
            return false;
        }

        Date now = new Date();
        task.setWrkSts(2L);
        task.setRgvNo(rgv.shuttle.getRgvNo());
        task.setAppeTime(now);
        task.setCrnStrTime(now);
        wrkMastService.updateById(task);
        log.info("RGV命令下发成功，RGV号={}，任务号={}，source={}，target={}",
                rgv.shuttle.getRgvNo(), task.getWrkNo(), task.getSourceStaNo(), task.getStaNo());
        return true;
    }

    private void issueRoamTask(RgvRuntimeState rgv, long sourcePos) {
        if (rgv == null || rgv.protocol == null) {
            return;
        }
        if (rgv.protocol.getStatusType() != RgvStatusType.IDLE) {
            return;
        }
        long destination = getRoamDestination(sourcePos);
        if (!MessageQueue.offer(SlaveType.Rgv, rgv.shuttle.getRgvNo(), new Task(5, destination))) {
            log.error("RGV漫游任务下发失败，RGV号={}，目的地={}", rgv.shuttle.getRgvNo(), destination);
            return;
        }
        log.info("RGV漫游任务下发成功，RGV号={}，目的地={}", rgv.shuttle.getRgvNo(), destination);
    }

    private long getRoamDestination(long sourcePos) {
        long destination = sourcePos - taskRunPerimeter + 10000;
        if (destination > perimeter) {
            return 10000L;
        }
        if (destination <= 0) {
            destination = perimeter + destination;
        }
        return destination;
    }

    private Map<Integer, Double> parseRegionPriorityMap() {
        String raw = regionPriorityValues == null ? "" : regionPriorityValues.trim();
        if (raw.equals(cachedRegionPriorityRaw)) {
            return cachedRegionPriorityMap;
        }
        Map<Integer, Double> regionMap = new HashMap<>();
        if (raw.isEmpty()) {
            cachedRegionPriorityRaw = raw;
            cachedRegionPriorityMap = regionMap;
            return regionMap;
        }
        String[] items = raw.split(",");
        for (String item : items) {
            if (item == null || item.trim().isEmpty()) {
                continue;
            }
            String[] pair = item.split(":");
            if (pair.length != 2) {
                continue;
            }
            try {
                Integer region = Integer.valueOf(pair[0].trim());
                Double priority = Double.valueOf(pair[1].trim());
                if (priority > 1D) {
                    priority = 1D;
                } else if (priority < -1D) {
                    priority = -1D;
                }
                regionMap.put(region, priority);
            } catch (Exception ignore) {
            }
        }
        cachedRegionPriorityRaw = raw;
        cachedRegionPriorityMap = regionMap;
        return regionMap;
    }

    private double[] resolveWeights(int pendingTaskCount, int availableRgvCount) {
        double timeW = priorityTimeWeight == null ? 0.4D : priorityTimeWeight;
        double distanceW = priorityDistanceWeight == null ? 0.4D : priorityDistanceWeight;
        double regionW = priorityRegionWeight == null ? 0.2D : priorityRegionWeight;

        double capacity = Math.max(1D, availableRgvCount * 2D);
        double loadFactor = Math.min(1D, pendingTaskCount / capacity);
        if (loadFactor > 0.7D) {
            timeW = Math.min(0.55D, timeW + 0.10D);
            distanceW = Math.max(0.10D, distanceW - 0.05D);
        } else if (loadFactor < 0.4D) {
            timeW = Math.max(0.25D, timeW - 0.05D);
            distanceW = Math.min(0.55D, distanceW + 0.05D);
        }

        double sum = Math.abs(timeW) + Math.abs(distanceW) + Math.abs(regionW);
        if (sum <= 0D) {
            return new double[]{0.4D, 0.4D, 0.2D};
        }
        return new double[]{timeW / sum, distanceW / sum, regionW / sum};
    }

    private static class RgvRuntimeState {
        private final BasCircularShuttle shuttle;
        private final RgvProtocol protocol;

        private RgvRuntimeState(BasCircularShuttle shuttle, RgvProtocol protocol) {
            this.shuttle = shuttle;
            this.protocol = protocol;
        }
    }

    //获取小车取放任务列表失败
    public synchronized List<List<List<WrkMast>>> getWrkMastListAll(List<List<Integer>> siteListAll ,BasDevpPosition[] basDevpPositionsListUN) {
        List<List<List<WrkMast>>> wrkMastListAll = new ArrayList<>();
        List<Integer> sourceStaNoList = new ArrayList<>();

        for (List<Integer> siteList : siteListAll){
            List<List<WrkMast>> wrkMastLists = new ArrayList<>();
            try {
                List<WrkMast> wrkMastlistA = new ArrayList<>();
                List<WrkMast> wrkMastlistB = new ArrayList<>();
                List<WrkMast> wrkMastlistC = new ArrayList<>();
                List<WrkMast> wrkMastlistD = new ArrayList<>();
                for (BasDevpPosition basDevpPosition : basDevpPositionsListUN) {
                    List<WrkMast> wrkMastList = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("sta_no", basDevpPosition.getDevNo()).eq("wrk_sts", 1L).orderBy("modi_time", true));
                    for (WrkMast wrkMast : wrkMastList) {
                        if (!Cools.isEmpty(wrkMast)) {
                            if (siteList.contains(wrkMast.getSourceStaNo())){
                                if (!sourceStaNoList.contains(wrkMast.getSourceStaNo())){
                                    List<WrkMast> wrkMastListS = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("source_sta_no", wrkMast.getSourceStaNo()).eq("wrk_sts", 1L).orderBy("modi_time", true));
                                    if (wrkMastListS.isEmpty()){
                                        continue;
                                    }
                                } else {
                                    continue;
                                }
                                long differenceInSeconds = TimeCalculatorUtils.differenceInMilliseconds(wrkMast.getModiTime(), new Date());

                                if (SortTheExecutionOfTheCarUtil.devpNoSortbj(basDevpPositionsListUN, wrkMast.getSourceStaNo(), wrkMast.getStaNo())) {
                                    sourceStaNoList.add(wrkMast.getSourceStaNo());
                                    if (differenceInSeconds>600000){
                                        wrkMastlistA.add(wrkMast);
                                    } else {
                                        wrkMastlistC.add(wrkMast);
                                    }
                                } else {
                                    sourceStaNoList.add(wrkMast.getSourceStaNo());
                                    if (differenceInSeconds>600000){
                                        wrkMastlistB.add(wrkMast);
                                    } else {
                                        wrkMastlistD.add(wrkMast);
                                    }
                                }
                            }
                        }
                    }
                }
                wrkMastLists.add(wrkMastlistA);
                wrkMastLists.add(wrkMastlistB);
                wrkMastLists.add(wrkMastlistC);
                wrkMastLists.add(wrkMastlistD);
//                return wrkMastLists;
                wrkMastListAll.add(wrkMastLists);
            } catch (Exception e) {
                log.error("获取小车取放任务列表失败，异常：" + e);
            }
        }
        return wrkMastListAll;
    }

    //获取小车取放任务列表失败
@@ -280,143 +601,185 @@
        }
    }

    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;
                    }
    public synchronized void taskDown(List<List<List<WrkMast>>> wrkMastListAll) {
        List<Integer> rgvNoM = new ArrayList<>();
        List<Integer> rgvNoR = new ArrayList<>();
        int wrkMastRunCount = 0;
        long devperimeter = 0L;
        try{
            for (List<List<WrkMast>> wrkMastLists : wrkMastListAll) {
                List<Integer> rgvNoRN = new ArrayList<>();
                runRgv:
                for (List<WrkMast> wrkMastList : wrkMastLists) {
                    for (WrkMast wrkMast : wrkMastList) {

                    List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("rgv_no", rgvProtocol.getRgvNo()).eq("wrk_sts", 2L));
                    if (!wrkMasts.isEmpty()) {
                        continue;
                    }
                    if (rgvProtocol != null
                            && 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;
//                                }
                            }
                        int[][] stePositionNearby = getStePositionNearby(wrkMast.getSourceStaNo());
                        if (stePositionNearby==null){
                            break  runRgv;
                        }
                        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.getSourceStaNo().shortValue());   //工位1起点
                        rgvCommand.setDestinationStaNo1(wrkMast.getStaNo().shortValue());   //工位1目标站点
                        rgvCommand.setCommand((short) 1);   //工位1任务确认
                        rgvCommand.setRgvSome(basCircularShuttle.getRgvSome()==1 ? (short) 1 : (short) 0);
                        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.setWrkSts(2L);
                        wrkMast.setRgvNo(rgvProtocol.getRgvNo());
                        Date now = new Date();
                        wrkMast.setAppeTime(now);
                        wrkMast.setCrnStrTime(now);
                        try {
                            wrkMastService.updateById(wrkMast);
                            break;
                        } catch (Exception e) {
                            log.error("更新小车任务失败,任务号：" + wrkMast.getWrkNo());
                        int[] rgvListRun = new int[stePositionNearby.length];
                        for (int[] rgvNo:stePositionNearby){
                            rgvListRun[rgvNo[0]-1] = rgvNo[1];
                        }
                        break runRgv;
                    } else if (rgvProtocol != null
                            && rgvProtocol.modeType == RgvModeType.AUTO
                            && rgvProtocol.getStatusType() == RgvStatusType.WALK
                            && rgvProtocol.getTaskNo1() == 0
                            && rgvProtocol.getAlarm() == 0) {
                        if (rgvProtocol.getStatusType() == RgvStatusType.WALK) {
                            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())){

                        for (int rgvNo : rgvListRun){
                            if (rgvNoRN.contains(rgvNo)){
                                continue;
                            }

                            BasCircularShuttle basCircularShuttle = basCircularShuttleService.selectOne(new EntityWrapper<BasCircularShuttle>().eq("rgv_no", rgvNo));
                            if (basCircularShuttle.getStatus() != 0){
                                continue ;
                            }
                            long rgvId = basCircularShuttle.getRgvId();

                            RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, basCircularShuttle.getRgvNo());
                            RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                            if (rgvProtocol == null) {
                                continue;
                            }

                            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("rgv_no", rgvProtocol.getRgvNo()).eq("wrk_sts", 2L));
                            if (!wrkMasts.isEmpty()) {
                                continue;
                            }
                            if (rgvProtocol != null
                                    && rgvProtocol.modeType == RgvModeType.AUTO
                                    && (rgvProtocol.getStatusType() == RgvStatusType.IDLE || rgvProtocol.getStatusType() == RgvStatusType.ROAM)
                                    && rgvProtocol.getTaskNo1() == 0
                                    && rgvProtocol.getRgvPosInt() != 0
                                    && rgvProtocol.getAlarm() == 0) {
                                BasDevpPosition basDevpPosition = basDevpPositionService.selectOne(new EntityWrapper<BasDevpPosition>().eq("dev_no", wrkMast.getSourceStaNo()));
                                if (basDevpPosition.getPlcPosition()>rgvProtocol.getRgvPos()){
                                    if (basDevpPosition.getPlcPosition()-rgvProtocol.getRgvPos()>taskRunPerimeter){
                                        if (!rgvNoM.contains(rgvProtocol.getRgvNo())){
                                            rgvNoM.add(rgvProtocol.getRgvNo());
                                            rgvNoRN.add(rgvProtocol.getRgvNo());
                                            if (wrkMastRunCount == 0){
                                                devperimeter = basDevpPosition.getPlcPosition()-taskRunPerimeter+10000>perimeter? 10000:basDevpPosition.getPlcPosition()-taskRunPerimeter+10000;
                                            }
                                            wrkMastRunCount++;
                                            break;
                                        }
                                        continue ;
                                    }
                                } else {
                                    if (rgvProtocol.getRgvPos()+ perimeter -basDevpPosition.getPlcPosition()>taskRunPerimeter){
                                        if (!rgvNoM.contains(rgvProtocol.getRgvNo())){
                                            rgvNoM.add(rgvProtocol.getRgvNo());
                                            rgvNoRN.add(rgvProtocol.getRgvNo());
                                            if (wrkMastRunCount == 0){
                                                devperimeter = basDevpPosition.getPlcPosition()-taskRunPerimeter+10000>perimeter? 10000:basDevpPosition.getPlcPosition()-taskRunPerimeter+10000;
                                            }
                                            wrkMastRunCount++;
                                            break;
                                        }
                                        continue ;
                                    }
                                }

                                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);
                                    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;
//                                }
                                    }
                                }

                                rgvNoRN.add(rgvProtocol.getRgvNo());
                                if (!rgvNoR.contains(rgvProtocol.getRgvNo())){
                                    rgvNoR.add(rgvProtocol.getRgvNo());
                                } else {
                                    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.getSourceStaNo().shortValue());   //工位1起点
                                rgvCommand.setDestinationStaNo1(wrkMast.getStaNo().shortValue());   //工位1目标站点
                                rgvCommand.setCommand((short) 1);   //工位1任务确认
                                rgvCommand.setRgvSome(basCircularShuttle.getRgvSome()==1 ? (short) 1 : (short) 0);
                                if (!MessageQueue.offer(SlaveType.Rgv, rgvProtocol.getRgvNo(), new Task(2, rgvCommand))) {
                                    //
                                    log.error("RGV命令下发失败，RGV号={}，任务数据={}", rgvId, JSON.toJSON(rgvCommand));
                                    wrkMast.setLogErrTime(new Date());
                                    wrkMast.setLogErrMemo("RGV命令下发失败，RGV号={"+rgvId+"}===>跳过");
                                    wrkMastService.updateById(wrkMast);
                                    break runRgv;
                                }
                                log.info("RGV命令下发成功，RGV号={}，任务数据={}", rgvId, JSON.toJSON(rgvCommand));

                                wrkMast.setWrkSts(2L);
                                wrkMast.setRgvNo(rgvProtocol.getRgvNo());
                                Date now = new Date();
                                wrkMast.setAppeTime(now);
                                wrkMast.setCrnStrTime(now);
                                try {
                                    wrkMastService.updateById(wrkMast);
                                    break;
                                } catch (Exception e) {
                                    log.error("更新小车任务失败,任务号：" + wrkMast.getWrkNo());
                                }
                                break runRgv;
                            }
                        }
                        RgvCommand rgvCommand = new RgvCommand();
                        rgvCommand.setRgvNo(rgvProtocol.getRgvNo());
                        rgvCommand.setAckFinish1((short) 0);  // 工位1任务完成确认位
                        rgvCommand.setTaskNo1(wrkMast.getWrkNo().shortValue());
                        rgvCommand.setTaskMode1(RgvTaskModeType.X_MOVE); // 工位1任务模式:  取放货
                        rgvCommand.setSourceStaNo1(wrkMast.getSourceStaNo().shortValue());   //工位1起点
                        rgvCommand.setDestinationStaNo1(wrkMast.getStaNo().shortValue());   //工位1目标站点
                        rgvCommand.setCommand((short) 1);   //工位1任务确认
                        rgvCommand.setRgvSome(basCircularShuttle.getRgvSome()==1 ? (short) 1 : (short) 0);
                        if (!MessageQueue.offer(SlaveType.Rgv, rgvProtocol.getRgvNo(), new Task(3, rgvCommand))) {
                            //
                            log.error("RGV命令下发失败，RGV号={}，任务数据={}", rgvId, JSON.toJSON(rgvCommand));
                            break runRgv;
                        }
                        log.info("RGV命令下发成功，RGV号={}，任务数据={}", rgvId, JSON.toJSON(rgvCommand));

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

        } catch (Exception e){
            log.error("RGV取放货启动命令下发失败，任务数据===> "+JSON.toJSON(wrkMastListAll));
            return;
        }
        try{
            for (Integer rgvNo : rgvNoM){
                if (wrkMastRunCount==0){
                    break;
                }
                if (rgvNoR.contains(rgvNo)){
                    continue;
                }
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvNo);
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
                    continue;
                }
                wrkMastRunCount--;
                if (rgvProtocol != null
                        && rgvProtocol.modeType == RgvModeType.AUTO
                        && rgvProtocol.getStatusType() == RgvStatusType.IDLE
                        && rgvProtocol.getTaskNo1() == 0
                        && rgvProtocol.getAlarm() == 0) {
                    if (!MessageQueue.offer(SlaveType.Rgv, rgvProtocol.getRgvNo(), new Task(5,devperimeter))) {
                        log.error("RGV漫游启动命令下发失败，RGV号={}，任务数据={}", rgvNo, "漫游指令，目的地："+devperimeter);
                        break;
                    } else {
                        log.info("RGV漫游启动命令下发成功，RGV号={}，任务数据={}", rgvNo, "漫游指令，目的地："+devperimeter);
                    }
                }
            }
        } catch (Exception e){

        }
    }

    //任务完成
    public synchronized void rgvCompleteWrkMastSta(int[] autoZ) {
        try {
            for (RgvSlave rgvSlave : slaveProperties.getRgv()) {
        for (RgvSlave rgvSlave : slaveProperties.getRgv()) {
            try {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvSlave.getId());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
@@ -427,7 +790,7 @@
                    log.error("{}号RGV尚未在数据库进行维护！", rgvSlave.getId());
                    continue;
                }
                if (rgvProtocol.getStatusType() == RgvStatusType.WAITING) {
                if (rgvProtocol.getStatusType() == RgvStatusType.WAITING || rgvProtocol.getStatusType() == RgvStatusType.WAITING101) {
                    log.info("{}号小车等待wcs确认，状态{}，参数{}", rgvProtocol.getRgvNo(), rgvProtocol.getStatusType(), rgvProtocol);
                }
                // 只有当RGV等待WCS确认、自动
@@ -436,7 +799,7 @@
                        && rgvProtocol.getTaskNo1() != 0
                ) {
//                    log.info("{}号小车等待wcs确认，状态{}，参数{}",rgvProtocol.getRgvNo(),rgvProtocol.getStatusType(),rgvProtocol);
                    WrkMast wrkMast = wrkMastService.selectOne(new EntityWrapper<WrkMast>().eq("wrk_no", rgvProtocol.getTaskNo1().longValue()));
                    WrkMast wrkMast = wrkMastService.selectOne(new EntityWrapper<WrkMast>().eq("wrk_no", rgvProtocol.getTaskNo1().longValue()).eq("rgv_no",rgvProtocol.getRgvNo()));
                    if (Cools.isEmpty(wrkMast) || !wrkMast.getWrkSts().equals(2L)) {
                        log.error("未查到小车执行任务或者执行任务状态不符合！" + wrkMast);
                        continue;
@@ -461,7 +824,7 @@
                                    wrkMastService.updateById(wrkMast);
                                    continue;
                                }else {
                                    log.error("工作号"+wrkMast.getWrkNo()+"任务完成信息上传处理失败");
                                    log.error("工作号"+wrkMast.getWrkNo()+"任务完成信息上传处理失败1");
                                }
                            } else {
                                //测试用
@@ -488,10 +851,77 @@
                        }
                    } catch (Exception e){}
                    wrkMastService.updateById(wrkMast);
                } else if (rgvProtocol.getStatusType() == RgvStatusType.WAITING101
                        && rgvProtocol.getModeType() == RgvModeType.AUTO
                        && rgvProtocol.getTaskNo1() != 0
                ){
                    boolean rgvComplete = rgvComplete(rgvProtocol.getRgvNo());
                    if (!rgvComplete) {
                        log.error("小车复位失败，小车号{}！", rgvProtocol.getRgvNo());
                        break;
                    }
                }
            } catch (Exception e) {
                log.error("小车复位线程报错！" + e);
            }
        } catch (Exception e) {
            log.error("小车复位线程报错！" + e);
        }
    }
    public synchronized void rgvCancelWrkMastSta() {
        for (RgvSlave rgvSlave : slaveProperties.getRgv()) {
            try {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvSlave.getId());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
                    continue;
                }
                if (rgvProtocol.getStatusType() == RgvStatusType.CANCEL) {
                    log.info("{}号小车等待wcs取消，状态{}，参数{}", rgvProtocol.getRgvNo(), rgvProtocol.getStatusType(), rgvProtocol);
                }
                // 只有当RGV等待WCS确认、自动
                if (rgvProtocol.getStatusType() == RgvStatusType.CANCEL
                        && rgvProtocol.getModeType() == RgvModeType.AUTO
                        && rgvProtocol.getTaskNo1() != 0
                ) {

                    RgvCommand rgvCommand = new RgvCommand();
                    rgvCommand.setRgvNo(rgvProtocol.getRgvNo());
                    rgvCommand.setAckFinish1((short) 0);  // 工位1任务完成确认位
                    rgvCommand.setTaskNo1(rgvProtocol.getTaskNo1());
                    rgvCommand.setTaskMode1(RgvTaskModeType.CANCEL); // 工位1任务模式:  取消
                    rgvCommand.setSourceStaNo1((short)-1);   //工位1起点
                    rgvCommand.setDestinationStaNo1((short)-1);   //工位1目标站点
//                    rgvCommand.setCommand((short) 1);   //工位1任务确认
//                    rgvCommand.setRgvSome((short) 1);
                    if (!MessageQueue.offer(SlaveType.Rgv, rgvProtocol.getRgvNo(), new Task(4, rgvCommand))) {
                        //
                        log.error("RGV取消命令下发失败，RGV号={}，任务数据={}", rgvProtocol.getRgvNo(), JSON.toJSON(rgvCommand));
//                        wrkMast.setLogErrTime(new Date());
//                        wrkMast.setLogErrMemo("RGV取消命令下发失败，RGV号={"+rgvProtocol.getRgvNo()+"}===>跳过");
//                        wrkMastService.updateById(wrkMast);
                        continue;
                    }
                    log.info("RGV取消命令下发成功，RGV号={}，任务数据={}", rgvProtocol.getRgvNo(), JSON.toJSON(rgvCommand));

                    WrkMast wrkMast = wrkMastService.selectOne(new EntityWrapper<WrkMast>().eq("wrk_no", rgvCommand.getTaskNo1().longValue()).eq("rgv_no",rgvCommand.getRgvNo()));
                    if (Cools.isEmpty(wrkMast) || !wrkMast.getWrkSts().equals(2L)) {
                        log.error("未查到小车执行任务或者执行任务状态不符合！" + wrkMast);
                        continue;
                    }

                    Date now = new Date();
                    wrkMast.setWrkSts(1L);
                    wrkMast.setRgvNo(0);
                    wrkMast.setAppeTime(now);
                    wrkMast.setLogErrTime(now);

                    wrkMast.setLogErrMemo("RGV申请取消任务，RGV号={"+rgvProtocol.getRgvNo()+"}，取消时间："+now);

                    wrkMastService.updateById(wrkMast);

                }
            } catch (Exception e) {
                log.error("小车取消线程报错！" + e);
            }
        }
    }