#改造

cpT

2025-07-02 15154033d40f857f61760f137ca59eaf29908aa3

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

@@ -1,8 +1,16 @@
package com.zy.asrs.utils;

import com.baomidou.mybatisplus.mapper.EntityWrapper;
import com.core.common.SpringUtils;
import com.zy.asrs.entity.BasDevpPosition;
import com.zy.asrs.entity.TaskWrk;
import com.zy.asrs.entity.WrkMast;
import com.zy.core.enums.RouteCollectCountType;
import com.zy.core.model.RgvSlave;
import com.zy.system.service.UserService;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

import static java.util.stream.Collectors.toList;
@@ -11,441 +19,174 @@
 * Created by Monkey D. Luffy on 2023/7/18
 */
public class RouteUtils {
    //排序  执行方向（面朝轨道 定位值左小右大）  0:左 小   1:右 大
    public static List<Integer>[] gradeRange(List<Integer> staNoList, List<BasDevpPosition> basDevpPositionList, boolean itSmall) {
        List<Integer>[] avoidRangeArray = new ArrayList[2];

    public static final List<Integer> MERGE_STA_NO_One = new ArrayList<Integer>() {{
        add(171);add(116);add(117);
    }};
        Integer[] rangeList = new Integer[staNoList.size()];
        List<Integer> rangeList1 = new ArrayList<>();
        List<Integer> rangeList2 = new ArrayList<>();

    public static final List<Integer> MERGE_STA_NO_TWO = new ArrayList<Integer>() {{
        add(113);add(123);add(114);add(1142);
    }};

    public static final List<Integer> MERGE_STA_NO_THREE = new ArrayList<Integer>() {{
        add(107);add(118);add(108);add(119);
    }};

    public static final List<Integer> MERGE_STA_NO_FOUR = new ArrayList<Integer>() {{
        add(110);add(120);add(111);add(121);
    }};

    public static final List<Integer> MERGE_STA_NO_FIVE = new ArrayList<Integer>() {{
        add(104);add(122);add(105);
    }};

    public static final List<Integer> MERGE_STA_NO_SIX = new ArrayList<Integer>() {{
        add(108);add(119);add(110);add(120);
    }};

    public static final List<Integer> MERGE_STA_NO_SEVEN = new ArrayList<Integer>() {{

    }};

    public static final List<Integer> MERGE_STA_NO_EIGHT = new ArrayList<Integer>() {{

    }};

    public static final List<Integer> MERGE_STA_NO_NINE = new ArrayList<Integer>() {{

    }};

    //    正序
    public static final List<Integer> TRACK_POSITION_POSITIVE_SEQUENCE1 = new ArrayList<Integer>() {{
        add(101);add(102);add(104);add(122);add(105);add(107);add(118);add(108);
    }};
    //    反序
    public static final List<Integer> TRACK_POSITION_REVERSE_SEQUENCE2 = new ArrayList<Integer>() {{
        add(117);add(116);add(171);add(1142);add(114);add(123);add(113);add(121);add(111);add(120);add(110);
    }};

    public static Integer getEndRoute(Integer endRoute,Integer rgvNo){
        if (rgvNo == 1){
            if (TRACK_POSITION_POSITIVE_SEQUENCE1.contains(endRoute)){
                return endRoute;
            } else {
                return 108;
            }
        } else if (rgvNo == 2){
            if (TRACK_POSITION_REVERSE_SEQUENCE2.contains(endRoute)){
                return endRoute;
            } else {
                return 110;
            }
        }
        return endRoute;
    }

//    正序
    public static final List<Integer> TRACK_POSITION_POSITIVE_SEQUENCE = new ArrayList<Integer>() {{
        add(101);add(102);add(104);add(122);add(105);add(107);add(118);add(108);add(119);add(110);add(120);add(111);add(121);add(113);add(123);add(114);add(1142);add(171);add(116);add(117);
    }};
//    反序
    public static final List<Integer> TRACK_POSITION_REVERSE_SEQUENCE = new ArrayList<Integer>() {{
        add(117);add(116);add(171);add(1142);add(114);add(123);add(113);add(121);add(111);add(120);add(110);add(119);add(108);add(118);add(107);add(105);add(122);add(104);add(102);add(101);
    }};

    public static String zerofill(String msg, Integer count){
        if (msg.length() == count){
            return msg;
        } else if (msg.length() > count){
            return msg.substring(0, 16);
        } else {
            StringBuilder msgBuilder = new StringBuilder(msg);
            for (int i = 0; i<count-msg.length(); i++){
                msgBuilder.insert(0,"0");
            }
            return msgBuilder.toString();
        }
    }
//    /*
//     * 获取干涉最远站点
//     * */
//    public static Integer RouteIndexFarMas(int rgvNo,Integer staNo){
//        List<Integer> staFall = new ArrayList<>();
//        if (MERGE_STA_NO_SIX.contains(staNo)){
//            staFall = MERGE_STA_NO_SIX;
//        }else if (MERGE_STA_NO_One.contains(staNo)){
//            staFall = MERGE_STA_NO_One;
//        }else if (MERGE_STA_NO_TWO.contains(staNo)){
//            staFall = MERGE_STA_NO_TWO;
//        }else if (MERGE_STA_NO_THREE.contains(staNo)){
//            staFall = MERGE_STA_NO_THREE;
//        }else if (MERGE_STA_NO_FOUR.contains(staNo)){
//            staFall = MERGE_STA_NO_FOUR;
//        }else if (MERGE_STA_NO_FIVE.contains(staNo)){
//            staFall = MERGE_STA_NO_FIVE;
//        }else {
//            return staNo;
//        }
//
//        int indexNo = 0;
//        if (rgvNo==1){
//            indexNo=staFall.size()-1;
//        }
//        return staFall.get(indexNo);
//    }

    /*
     * 获取干涉最远站点
     * */
    public static Integer RouteIndexFarMas(int rgvNo,Integer staNo){
        if (rgvNo==1){
            switch (staNo){
                case 101:
                case 102: return  102;
                case 104:
                case 122: return  118;
                case 105: return  119;
                case 107:
                case 118: return  108;
                case 108:
                case 119: return  111;
                case 110:
                case 120: return  113;
                case 111:
                case 121: return  113;
                case 113: return  114;
                case 123:
                case 114: return  1142;
                default:
                    return 116;
            }
        }else {
            switch (staNo){
                case 117:
                case 116:
                case 171:
                    return  1142;
                case 1142:
                case 114: return  113;
                case 123:
                case 113: return  111;
                case 121: return  110;
                case 111: return  110;
                case 120:
                case 110: return 108;
                case 119:
                case 108: return  107;
                case 118:
                case 107: return  105;
                case 105:
                case 122:
                case 104: return  104;
                default:
                    return 102;
            }
        }
    }
    /*
     * 获取干涉最远站点
     * */
    public static Integer RouteIndexFarMasL(int rgvNo,Integer staNo){
        if (rgvNo==1){
            switch (staNo){
                case 101:
                case 102:
                case 104:
                case 122:
                case 105:
                case 107:
                    return 118;
                case 118:
                    return staNo;
                default:
                    return 108;
            }
        }else {
            switch (staNo){
                case 117:
                case 116:
                case 171:
                case 1142:
                case 114:
                case 123:
                case 113:
                case 121:
                    return 121;
                case 111:
                    return staNo;
                default:
                    return 110;
            }
        }
    }

    // 获取当前小车未行走的路线集合
    public static List<Integer> getRoute(Integer groupStart,Integer groupEnd){
        boolean sign = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(groupStart) < TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(groupEnd);
        List<Integer> result = new ArrayList<>();
        List<Integer> groupRoute = new ArrayList<>();
        if (sign){
            groupRoute = TRACK_POSITION_POSITIVE_SEQUENCE;
        }else {
            groupRoute = TRACK_POSITION_REVERSE_SEQUENCE;
        }
        if (groupRoute.contains(groupStart) && groupRoute.contains(groupEnd)) {
            sign = false;
            for (Integer route : groupRoute) {
                if (route.equals(groupStart)){
                    sign=true;
                }
                if (route.equals(groupEnd)){
                    result.add(route);
        int i = 0;
        for (BasDevpPosition basDevpPosition : basDevpPositionList) {
            for (Integer staNo : staNoList) {
                if (basDevpPosition.getDevNo().equals(staNo)) {
                    rangeList[i] = staNo;
                    i = i + 1;
                    break;
                }
                if (sign){
                    result.add(route);
                }
            }
        }else {
            return null;
        }
        return result;
    }

    /*
    * 获取最远站点
    * */
    public static Integer RouteIndexFarMas(Integer a,Integer b,Integer c,Integer d){
        int ia = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(a);
        int ib = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(b);
        int ic = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(c);
        int id = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(d);
        if (id==0){
            if (ia>ib){
                if (ia>ic){
                    return a;
                }else {
                    return c;
        boolean sign = true;
        for (int j = 0; j < rangeList.length; j++) {
            if (itSmall) {
                if (sign) {
                    rangeList1.add(rangeList[j]);
                } else {
                    rangeList2.add(rangeList[j]);
                }
            }else {
                if (ib>ic){
                    return b;
                }else {
                    return c;
                if (sign && j >= rangeList.length / 2) {
                    sign = false;
                }
            }
        }else {
            if (ia<ib){
                if (ia<ic){
                    return a;
                }else {
                    return c;
            } else {
                if (sign && j >= rangeList.length / 2) {
                    sign = false;
                }
            }else {
                if (ib<ic){
                    return b;
                }else {
                    return c;
                if (sign) {
                    rangeList1.add(rangeList[j]);
                } else {
                    rangeList2.add(rangeList[j]);
                }
            }
        }

        avoidRangeArray[0] = rangeList1;
        avoidRangeArray[1] = rangeList2;
        return avoidRangeArray;
    }

    /*
     * 获取最远站点
     * a:当前位置
     * b:开始位置
     * c:结束位置
     * d:锁开始位置
     * */
    public static Integer RouteIndexFarMasNew(Integer a,Integer b,Integer c,Integer d){
        int ia = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(a);
        int ib = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(b);
        int ic = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(c);
        int id = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(d);
        if (id==0){
            if (ia>ib){
                if (ia>ic){
                    return a;
                }else {
                    return c;
                }
            }else {
                if (ib>ic){
                    return b;
                }else {
                    return c;
                }
            }
        }else {
            if (ia<ib){
                if (ia<ic){
                    return a;
                }else {
                    return c;
                }
            }else {
                if (ib<ic){
                    return b;
                }else {
                    return c;

    //获取在范围的站点
    public static List<Integer> belongToRange(List<Integer> staNoList, Long[] avoid, List<BasDevpPosition> basDevpPositions) {
        List<Integer> siteList = new ArrayList<>();

        for (BasDevpPosition basDevpPosition : basDevpPositions) {
            if (new TrackRangeUtils().avoidRange(basDevpPosition.getPlcPosition(), avoid)) {
                for (Integer staNo : staNoList) {
                    if (basDevpPosition.getDevNo().equals(staNo)) {
                        siteList.add(staNo);
                        break;
                    }
                }
            }
        }

        return siteList;
    }

    public static Integer[] RouteIndexFarArr(Integer a,Integer b,Integer c,Integer d){
        int ia = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(a);
        int ib = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(b);
        int ic = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(c);
        int id = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(d);
        return new Integer[]{ia,ib,ic,id};
    }

    /*
     * a  小车当前位置
     * b  小车任务开始位置
     * c  小车任务结束位置
     * d  小车初始位置
     * 获取最大路径差值
     * */
    public static List<Integer>  RouteMapCurrentFar(Integer a,Integer b,Integer c,Integer d){
        Integer farStn = RouteIndexFarMas(a, b, c, d);
        return getRouteIntersection(TRACK_POSITION_POSITIVE_SEQUENCE,getRoute(farStn, d), RouteCollectCountType.DIFFERENCESET);
    }

    public static List<Integer>  RouteMapCurrentFar(Integer farStn,Integer d){
        return getRouteIntersection(TRACK_POSITION_POSITIVE_SEQUENCE,getRoute(farStn, d), RouteCollectCountType.DIFFERENCESET);
    }

    /*
    * 另一台小车最远位置
    * */
    public static Integer RouteMapOtherFarStnNo(List<Integer> routeDIFF,Integer d){
        int id = TRACK_POSITION_POSITIVE_SEQUENCE.indexOf(d);
        if (id==0){
            return routeDIFF.get(0);
        }else {
            int size = routeDIFF.size();
            return routeDIFF.get(size-1);
    //提取站点集合
    public static List<Integer> BasDevpPositionExtractSites(List<BasDevpPosition> basDevpPositions) {
        List<Integer> siteList = new ArrayList<>();
        for (BasDevpPosition basDevpPosition : basDevpPositions) {
            if (!siteList.contains(basDevpPosition.getDevNo())) {
                siteList.add(basDevpPosition.getDevNo());
            }
        }
        return siteList;
    }

    //是否有交集
    public static boolean getRouteBoolean(List<Integer> groupCurrent,List<Integer> groupOther){
        for (Integer positionCurrent : groupCurrent){
            for (Integer positionOther : groupOther){
                if (positionCurrent.equals(positionOther)){
                    return true;
    //提取站点集合//就近排序
    public static List<Integer> SortNearby(List<Integer> staNoList, Long rgvNowPos, List<BasDevpPosition> basDevpPositionList) {
        List<Integer> siteList = new ArrayList<>();

        List<BasDevpPosition> basDevpPositions = devpNoSort(basDevpPositionList, rgvNowPos);
        for (BasDevpPosition basDevpPosition : basDevpPositions) {
            for (Integer staNo : staNoList) {
                if (basDevpPosition.getDevNo().equals(staNo)) {
                    siteList.add(staNo);
                    break;
                }
            }
        }
        return false;


        return siteList;
    }

    //集合运算
    public static List<Integer> getRouteIntersection(List<Integer> groupCurrent, List<Integer> groupOther, RouteCollectCountType routeCollectCountType){
        switch (routeCollectCountType){
            case INTERSECTION:
                //交集
                return groupCurrent.stream().filter(item -> groupOther.contains(item)).collect(toList());
            case DIFFERENCESET:
                //差集
                return groupCurrent.stream().filter(item -> !groupOther.contains(item)).collect(toList());
            case UNION:
                //并集
                groupCurrent.addAll(groupOther);
                return groupCurrent;
            case DEDUPLICATIONUNION:
                //去重并集
                groupCurrent.addAll(groupOther);
                return groupCurrent.stream().distinct().collect(toList());
            default:
                return null;
    //站点过滤
    public static List<BasDevpPosition> devpNoSort(List<BasDevpPosition> devpPositionList, Long rgvNowPos) {

        List<BasDevpPosition> basDevpPositions = new ArrayList<>();
        List<BasDevpPosition> basDevpPositionSort = new ArrayList<>();
        ArrayList<Long> arrayList = new ArrayList<>();
        for (BasDevpPosition basDevpPosition : devpPositionList) {
            long position = Math.abs(basDevpPosition.getPlcPosition() - rgvNowPos);
            BasDevpPosition devpPosition = new BasDevpPosition(basDevpPosition, position);
            basDevpPositions.add(devpPosition);
            arrayList.add(position);
        }
    }

    /*
    * a  小车当前位置
    * b  小车任务开始位置
    * c  小车任务结束位置
    * sign  另一个小车空闲标记
    * */
    public static boolean RouteAutoBoolean(Integer a,Integer b,Integer c,Integer aa,Integer bb,Integer cc,boolean idleOther){
        List<Integer> routeCurrent = getRouteIntersection(getRoute(a, c), getRoute(b, c), RouteCollectCountType.DEDUPLICATIONUNION);//并集
        List<Integer> routeOther = getRouteIntersection(getRoute(aa, cc), getRoute(bb, cc), RouteCollectCountType.DEDUPLICATIONUNION);//
        return !getRouteBoolean(routeCurrent, routeOther);  //是否有交集
    }

    public static List<Integer> RouteAutoBoolean(Integer a,Integer b,Integer c){
        return getRouteIntersection(getRoute(a, c), getRoute(b, c), RouteCollectCountType.DEDUPLICATIONUNION);
    }



    public static void main(String[] arge){
        List<Integer> routeCurrent = getRoute(104, 119);  //获取当前小车路径
        List<Integer> routeOther = getRoute(117, 118);  //获取其它小车路径
        System.out.println("当前小车路径:\t"+routeCurrent);
        System.out.println("其它小车路径:\t"+routeOther);

        boolean routeBoolean = getRouteBoolean(routeCurrent, routeOther);  //是否有交集
        System.out.println("是否有交集:\t"+routeBoolean);

        List<Integer> routeIntersection = getRouteIntersection(routeCurrent, routeOther, RouteCollectCountType.INTERSECTION);//交集
        System.out.println("路径交集：\t"+routeIntersection);

        List<Integer> routeIntersection1 = getRouteIntersection(routeCurrent, routeOther, RouteCollectCountType.DIFFERENCESET);//差集
        System.out.println("路径差集：\t"+routeIntersection1);

        List<Integer> routeIntersection2 = getRouteIntersection(routeCurrent, routeOther, RouteCollectCountType.UNION);//并集
        System.out.println("路径并集：\t"+routeIntersection2);

        List<Integer> routeIntersection3 = getRouteIntersection(routeCurrent, routeOther, RouteCollectCountType.DEDUPLICATIONUNION);//去重并集
        System.out.println("路径去重并集：\t"+routeIntersection3);

        System.out.println(RouteAutoBoolean(101,104,119,114,116,120,true));

        List<Integer> routeDIFF = RouteMapCurrentFar(114, 104, 119, 117);
        System.out.println("当前路径最大集合：\t"+ routeDIFF);

        Integer routeFarOther= RouteMapOtherFarStnNo(routeDIFF,117);
        System.out.println("另一台小车最远站点：\t"+routeFarOther);


        Integer[] integers = RouteIndexFarArr(114, 104, 119, 101);
        for (Integer integer:integers){
            System.out.println(integer);
        Collections.sort(arrayList); // 升序排序
        for (Long position : arrayList) {
            for (BasDevpPosition basDevpPosition : basDevpPositions) {
                if (basDevpPosition.getPlcPosition().equals(position)) {
                    basDevpPositionSort.add(basDevpPosition);
                    basDevpPositions.remove(basDevpPosition);
                    break;
                }
            }
        }
        return basDevpPositionSort;
    }

    //检测是否在范围
    public static boolean CheckIfItIsWithinTheRange(List<Integer> staNoList, Long staNoNowPos, List<BasDevpPosition> basDevpPositionList, boolean itSmall) {
        List<Integer> siteList = new ArrayList<>();
        Long maxOrMin = 0L;


        Integer[] rangeList = new Integer[staNoList.size()];
        int i = 0;
        for (BasDevpPosition basDevpPosition : basDevpPositionList) {
            for (Integer staNo : staNoList) {
                if (basDevpPosition.getDevNo().equals(staNo)) {
                    rangeList[i] = staNo;
                    i = i + 1;
                    break;
                }
            }
        }
        if (itSmall) {
            for (BasDevpPosition basDevpPosition : basDevpPositionList) {
                if (basDevpPosition.getDevNo().equals(rangeList[rangeList.length - 1])) {
                    maxOrMin = basDevpPosition.getPlcPosition();
                    break;
                }
            }
            if (maxOrMin == 0){
                return false;
            }
            return staNoNowPos <= maxOrMin+50;
        }
        for (BasDevpPosition basDevpPosition : basDevpPositionList) {
            if (basDevpPosition.getDevNo().equals(rangeList[0])) {
                maxOrMin = basDevpPosition.getPlcPosition();
                break;
            }
        }
        if (maxOrMin == 0){
            return false;
        }
        return staNoNowPos >= maxOrMin-50;
    }

    public static long absoluteDifference(Long a, Long b) {
        if (a == null || b == null) {
            a = 0L;
            b = 0L;
//            throw new IllegalArgumentException("");
            System.out.println("a or b is null");
        }
        return Math.abs(a - b);
    }

}