#

Junjie

4 天以前 6daf900a09adcca981f620744bf89851654d88e0

 src/main/java/com/zy/core/thread/impl/TrafficControlImplThread.java

@@ -1,43 +1,35 @@
package com.zy.core.thread.impl;

import com.alibaba.fastjson.JSON;
import com.core.common.SpringUtils;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.zy.asrs.utils.Utils;
import com.zy.common.model.NavigateNode;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.common.utils.NavigatePositionConvert;
import com.zy.common.utils.NavigateUtils;
import com.zy.common.utils.RedisUtil;
import com.zy.common.utils.ShuttleOperaUtils;
import com.zy.core.News;
import com.zy.core.cache.SlaveConnection;
import com.zy.core.enums.RedisKeyType;
import com.zy.core.enums.SlaveType;
import com.zy.core.model.TrafficControlDataModel;
import com.zy.core.model.command.ShuttleAssignCommand;
import com.zy.core.model.command.ShuttleRedisCommand;
import com.zy.core.model.param.OperateTrafficControlParam;
import com.zy.core.model.protocol.ShuttleProtocol;
import com.zy.core.thread.ShuttleThread;
import com.zy.core.thread.TrafficControlThread;
import com.zy.core.utils.TrafficControlUtils;
import org.springframework.scheduling.annotation.Async;

import java.io.IOException;
import java.util.*;
import java.util.function.Function;
import java.util.stream.Collectors;

public class TrafficControlImplThread implements TrafficControlThread {

    private RedisUtil redisUtil;
    private boolean detecting = false;
    private boolean updateDetect = false;
    private Long detectTime = System.currentTimeMillis();
    private HashMap<String, Integer> deviceMap = null;
    private HashMap<Integer, HashMap<String, List<Integer>>> levNodesMap = null;


    private List<TrafficControlDataModel> applyList = new ArrayList<>();
    private HashMap<Integer,Long> shuttleReportErrorMap = new HashMap<>();
    private HashMap<Integer,Long> pathIdleShuttleMap = new HashMap<>();
    private HashMap<Integer,Long> applyRecordsMap = new HashMap<>();
    private HashMap<String, List<NavigateNode>> taskNodesMap = new HashMap<>();
    private List<TrafficControlDataModel> trafficControlDataList = new ArrayList<>();
    private List<TrafficControlDataModel> trafficControlDataListRead = new ArrayList<>();

    public TrafficControlImplThread(RedisUtil redisUtil) {
        this.redisUtil = redisUtil;
@@ -49,402 +41,61 @@
        Object object = redisUtil.get(RedisKeyType.TRAFFIC_CONTROL_MAP.key);
        if(object != null) {
            trafficControlDataList = (List<TrafficControlDataModel>) object;
            trafficControlDataListRead = trafficControlDataList.stream()
                    .map(model -> new TrafficControlDataModel(
                            model.getShuttleNo(),
                            model.getTaskNo(),
                            new ArrayList<>(model.getNodeList()),
                            new ArrayList<>(model.getTotalNodeList())
                    ))
                    .collect(Collectors.toList());
        }

//        List<Integer> shuttleNoList = new ArrayList<>();
//        while (true) {
//            try {
//                DeviceConfigService deviceConfigService = null;
//                try {
//                    deviceConfigService = SpringUtils.getBean(DeviceConfigService.class);
//                }catch (Exception e){}
//
//                if(deviceConfigService == null){
//                    continue;
//                }
//
//                if(shuttleNoList.isEmpty()){
//                    List<DeviceConfig> shuttleList = deviceConfigService.selectList(new EntityWrapper<DeviceConfig>()
//                            .eq("device_type", String.valueOf(SlaveType.Shuttle)));
//                    for (DeviceConfig deviceConfig : shuttleList) {
//                        shuttleNoList.add(deviceConfig.getDeviceNo());
//                    }
//                }
//
//                if((updateDetect) || ((System.currentTimeMillis() - detectTime) > 1000 * 2)) {
//                    detect(shuttleNoList);
//                }
//            }catch (Exception e){
//                e.printStackTrace();
//            }
//        }

    }

    public synchronized void detect(List<Integer> shuttleNoList) {
        detecting = true;
        updateDetect = false;
        ObjectMapper objectMapper = null;
        try {
            objectMapper = SpringUtils.getBean(ObjectMapper.class);
        }catch (Exception e){}

        if(objectMapper == null){
            return;
        }

        HashMap<String, List<NavigateNode>> tmpTaskNodesMap = new HashMap<>();
        for (Integer shuttleNo : shuttleNoList) {
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttleNo);
            if (shuttleThread == null) {
                continue;
            }

            ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
            if (shuttleProtocol == null) {
                continue;
            }

            if (shuttleProtocol.getTaskNo() == 0) {
                continue;
            }

            if (shuttleProtocol.getCurrentLocNo() == null) {
                continue;
            }

            int lev = Utils.getLev(shuttleProtocol.getCurrentLocNo());

            Object obj = redisUtil.get(RedisKeyType.SHUTTLE_WORK_FLAG.key + shuttleProtocol.getTaskNo());
            if (obj == null) {
                continue;
            }

            ShuttleRedisCommand redisCommand = null;
        while (true) {
            try {
                redisCommand = objectMapper.readValue(String.valueOf(obj), ShuttleRedisCommand.class);
            } catch (IOException e) {
                throw new RuntimeException(e);
            }
                List<TrafficControlDataModel> allTrafficControl = getAllTrafficControl();
                //更新交管信息
                redisUtil.set(RedisKeyType.TRAFFIC_CONTROL_MAP.key, allTrafficControl);

            if (redisCommand == null) {
                continue;
            }
                for (TrafficControlDataModel dataModel : allTrafficControl) {
                    Integer shuttleNo = dataModel.getShuttleNo();
                    Integer taskNo = dataModel.getTaskNo();

            ShuttleAssignCommand assignCommand = redisCommand.getAssignCommand();
            if (assignCommand == null) {
                continue;
            }

            List<NavigateNode> nodeList = assignCommand.getNodesDeepCopy();
            if (nodeList == null || nodeList.isEmpty()) {
                continue;
            }

            tmpTaskNodesMap.put(shuttleProtocol.getTaskNo() + "-" + shuttleNo, nodeList);
        }
        this.taskNodesMap = tmpTaskNodesMap;
        HashMap<Integer, HashMap<String, List<Integer>>> levNodesMap = calcNodeList();

        HashMap<Integer, HashMap<String, List<Integer>>> levBlockMap = new HashMap<>();
        //过滤不需要管制节点
        for (Map.Entry<Integer, HashMap<String, List<Integer>>> entry : levNodesMap.entrySet()) {
            Integer lev = entry.getKey();
            HashMap<String, List<Integer>> value = entry.getValue();

            HashMap<String, List<Integer>> blockMap = new HashMap<>();
            for (Map.Entry<String, List<Integer>> listEntry : value.entrySet()) {
                String locNo = listEntry.getKey();
                List<Integer> shuttleNos = listEntry.getValue();
                if (shuttleNos.size() <= 1) {
                    continue;
                }
                blockMap.put(locNo, shuttleNos);
            }

            levBlockMap.put(lev, blockMap);
        }

        //计算堵塞范围
        List<List<String>> allLocList = new ArrayList<>();
        List<List<Integer>> allDeviceNodes = new ArrayList<>();
        for (Map.Entry<Integer, HashMap<String, List<Integer>>> entry : levBlockMap.entrySet()) {
            Integer lev = entry.getKey();
            HashMap<String, List<Integer>> nodes = entry.getValue();

            Set<String> sets = new HashSet<>();
            for (Map.Entry<String, List<Integer>> val : nodes.entrySet()) {
                String locNo = val.getKey();
                sets.add(locNo);
            }

            List<List<String>> locList = TrafficControlUtils.groupNodes(sets);
            List<List<Integer>> deviceNodes = new ArrayList<>();

            //get devices
            for (List<String> list : locList) {
                List<List<Integer>> tmpDeviceNodes = new ArrayList<>();
                for (String loc : list) {
                    List<Integer> shuttleNos = nodes.get(loc);
                    if(!tmpDeviceNodes.contains(shuttleNos)) {
                        tmpDeviceNodes.add(shuttleNos);
                    Object object1 = redisUtil.get(RedisKeyType.TRAFFIC_CONTROL_SUCCESS_APPLY.key + shuttleNo + "_" + taskNo);
                    if(object1 == null) {
                        redisUtil.set(RedisKeyType.TRAFFIC_CONTROL_SUCCESS_APPLY.key + shuttleNo + "_" + taskNo, 1, 60 * 60);
                    }
                }
                //节点并集-获取堵塞设备编号
                List<List<Integer>> deviceList = mergeConnectedComponents(tmpDeviceNodes);
                deviceNodes.addAll(deviceList);
            }

            allLocList.addAll(locList);
            allDeviceNodes.addAll(deviceNodes);
        }

        System.out.println(JSON.toJSONString(allLocList));
        System.out.println(JSON.toJSONString(allDeviceNodes));
//        //分配堵塞节点可执行设备
//        findDeviceByBlockList(allLocList, allDeviceNodes);

        detecting = false;
        detectTime = System.currentTimeMillis();

//        //发布堵塞节点可用设备编号
//        redisUtil.set(RedisKeyType.TASK_BLOCK_ENABLE_DEVICE_MAP.key, trafficControlDataList);
    }

//    //分配堵塞节点可执行设备
//    public void findDeviceByBlockList(List<List<String>> allLocList, List<List<Integer>> blockNodes) {
//        HashMap<String, Integer> map = new HashMap<>();
//        if (deviceMap == null) {
//            Object object = redisUtil.get(RedisKeyType.TASK_BLOCK_ENABLE_DEVICE_MAP.key);
//            if (object != null) {
//                map = (HashMap<String, Integer>) object;
//            }
//        } else {
//            map = deviceMap;
//        }
//
//        HashMap<String, Integer> newMap = new HashMap<>();
//
//        for (int i = 0; i < blockNodes.size(); i++) {
//            List<Integer> blockNode = blockNodes.get(i);
//            List<String> locs = allLocList.get(i);
//
//            String key = JSON.toJSONString(locs);
//
//            Integer value = -1;
//            if (map.containsKey(key)) {
//                value = map.get(key);
//                map.remove(key);
//
//                if (value > 0) {
//                    ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, value);
//                    if (shuttleThread == null) {
//                        continue;
//                    }
//
//                    ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
//                    if (shuttleProtocol == null) {
//                        continue;
//                    }
//
//                    if (shuttleProtocol.getTaskNo() == 0) {
//                        value = searchDevice(locs, blockNode, newMap);
//                    }
//
//                    if (!shuttleProtocol.getTrafficControl()) {
//                        value = searchDevice(locs, blockNode, newMap);
//                    }
//                }else {
//                    value = searchDevice(locs, blockNode, newMap);
//                }
//            } else {
//                value = searchDevice(locs, blockNode, newMap);
//            }
//            newMap.put(key, value);
//        }
//
//        deviceMap = newMap;
//        //发布堵塞节点可用设备编号
//        redisUtil.set(RedisKeyType.TASK_BLOCK_ENABLE_DEVICE_MAP.key, newMap);
//    }

    public Integer searchDevice(List<String> locs, List<Integer> blockNode, HashMap<String, Integer> deviceMap) {
        NavigateUtils navigateUtils = null;
        try {
            navigateUtils = SpringUtils.getBean(NavigateUtils.class);
        }catch (Exception e){}

        if(navigateUtils == null){
            return -1;
        }

        Integer value = -1;
        for (Integer shuttleNo : blockNode) {
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttleNo);
            if (shuttleThread == null) {
                continue;
            }

            ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
            if (shuttleProtocol == null) {
                continue;
            }

            if (!shuttleThread.isDeviceIdle()) {
                continue;
            }

            if (shuttleProtocol.getTaskNo() == 0) {
                continue;
            }

            if (!shuttleProtocol.getTrafficControl()) {
                continue;//小车未申请交通管制
            }

            //检测小车是否已经分配
            if (deviceMap.containsValue(shuttleNo)) {
                continue;
            }

            List<NavigateNode> trafficControlNodes = shuttleProtocol.getTrafficControlNodes();
            List<String> trafficControlLocs = new ArrayList<>();
            for (NavigateNode node : trafficControlNodes) {
                trafficControlLocs.add(Utils.getLocNo(node.getX(), node.getY(), node.getZ()));
            }

            //检测当前小车节点是否匹配交通管制节点
            boolean result = false;
            for (String loc : locs) {
                if (trafficControlLocs.contains(loc)) {
                    result = true;
                    break;
                }
            }

            if (!result) {
                continue;
            }

            //check path
            String currentLocNo = shuttleProtocol.getCurrentLocNo();
            for (String loc : locs) {
                if (loc.equals(currentLocNo)) {
                    continue;
                }
                List<NavigateNode> nodeList = navigateUtils.calc(currentLocNo, loc, NavigationMapType.getNormalWithDevice(), Utils.getShuttlePoints(shuttleNo, Utils.getLev(loc)), null);
                if (nodeList == null) {
                    break;
                }
            }

            value = shuttleNo;
            break;
        }

        return value;
    }

    //节点并集
    public List<List<Integer>> mergeConnectedComponents(List<List<Integer>> lists) {
        // 1. 收集所有唯一元素
        Set<Integer> allElements = new HashSet<>();
        for (List<Integer> list : lists) {
            allElements.addAll(list);
        }

        // 2. 初始化并查集
        Map<Integer, Integer> parent = new HashMap<>();
        for (Integer element : allElements) {
            parent.put(element, element);
        }

        // 3. 定义查找根节点的函数（带路径压缩）
        Function<Integer, Integer> find = x -> {
            int root = x;
            while (parent.get(root) != root) {
                root = parent.get(root);
            }
            // 路径压缩
            int current = x;
            while (parent.get(current) != root) {
                int next = parent.get(current);
                parent.put(current, root);
                current = next;
            }
            return root;
        };

        // 4. 遍历每个列表并合并元素
        for (List<Integer> list : lists) {
            if (list.isEmpty()) continue;
            int first = list.get(0);
            for (int i = 1; i < list.size(); i++) {
                int a = first;
                int b = list.get(i);
                int rootA = find.apply(a);
                int rootB = find.apply(b);
                if (rootA != rootB) {
                    parent.put(rootB, rootA); // 合并集合
                }
                Thread.sleep(200);
            }catch (Exception e){
                e.printStackTrace();
            }
        }

        // 5. 按根节点分组
        Map<Integer, Set<Integer>> components = new HashMap<>();
        for (Integer element : allElements) {
            int root = find.apply(element);
            components.computeIfAbsent(root, k -> new TreeSet<>()).add(element);
        }

        // 6. 转换为有序列表
        List<List<Integer>> result = new ArrayList<>();
        for (Set<Integer> set : components.values()) {
            result.add(new ArrayList<>(set));
        }

        return result;
    }

    private HashMap<Integer, HashMap<String, List<Integer>>> calcNodeList() {
        HashMap<Integer, HashMap<String, List<Integer>>> levNodesMap = new HashMap<>();
        for (Map.Entry<String, List<NavigateNode>> entry : taskNodesMap.entrySet()) {
            String key = entry.getKey();
            String[] split = key.split("-");
            Integer taskNo = Integer.parseInt(split[0]);
            Integer shuttleNo = Integer.parseInt(split[1]);

            List<NavigateNode> nodeList = entry.getValue();
            NavigateNode node1 = nodeList.get(0);
            int lev = node1.getZ();

            HashMap<String, List<Integer>> nodeMap = new HashMap<>();
            if(levNodesMap.containsKey(lev)) {
                nodeMap = levNodesMap.get(lev);
            }

            for (NavigateNode node : nodeList) {
                String locNo = Utils.getLocNo(node.getX(), node.getY(), lev);
                List<Integer> shuttleNos = new ArrayList<>();
                if (nodeMap.containsKey(locNo)) {
                    shuttleNos = nodeMap.get(locNo);
                }

                if (!shuttleNos.contains(shuttleNo)) {
                    shuttleNos.add(shuttleNo);
                }
                nodeMap.put(locNo, shuttleNos);
            }
            levNodesMap.put(lev, nodeMap);
        }
        this.levNodesMap = levNodesMap;
        return levNodesMap;
    }

    @Override
    public synchronized boolean applyTrafficControl(List<NavigateNode> totalNodeList, List<NavigateNode> nodeList, Integer shuttleNo, Integer taskNo) {
        //更新交管信息
        redisUtil.set(RedisKeyType.TRAFFIC_CONTROL_MAP.key, trafficControlDataList);
    public synchronized boolean processApply(TrafficControlDataModel applyData) {
        ShuttleOperaUtils shuttleOperaUtils = SpringUtils.getBean(ShuttleOperaUtils.class);
        if (shuttleOperaUtils == null) {
            return false;
        }

        if (applyData == null) {
            return false;
        }

        if (applyData.getShuttleNo() == null || applyData.getTaskNo() == null || applyData.getNodeList() == null || applyData.getTotalNodeList() == null) {
            return false;
        }

        List<TrafficControlDataModel> allTrafficControlList = getAllTrafficControl();

        Integer shuttleNo = applyData.getShuttleNo();
        Integer taskNo = applyData.getTaskNo();
        List<NavigateNode> nodeList = applyData.getNodeList();
        List<NavigateNode> totalNodeList = applyData.getTotalNodeList();

        NavigateNode startNode = totalNodeList.get(0);
        List<int[]> shuttlePoints = Utils.getShuttlePoints(shuttleNo, startNode.getZ());
@@ -462,8 +113,8 @@
        }

        //检测车子是否存在管制
        for (int i = 0; i < trafficControlDataList.size(); i++) {
            TrafficControlDataModel controlDataModel = trafficControlDataList.get(i);
        for (int i = 0; i < allTrafficControlList.size(); i++) {
            TrafficControlDataModel controlDataModel = allTrafficControlList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
                //存在管制
                if(!controlDataModel.getTaskNo().equals(taskNo)) {
@@ -480,7 +131,7 @@
                    }
                }

                News.info("traffic running {},{}", shuttleNo, taskNo);
//                News.info("traffic running {},{}", shuttleNo, taskNo);
                return true;//已经管制允许执行
            }
        }
@@ -508,12 +159,36 @@
        //检测路径是否有小车
        for (String loc : totalLocList) {
            if(shuttleLocList.contains(loc)) {
                ShuttleThread pathShuttleThread = Utils.searchShuttle(loc);
                if (pathShuttleThread == null) {
                    return false;
                }

                ShuttleProtocol shuttleProtocol = pathShuttleThread.getStatus();
                if(shuttleProtocol == null) {
                    return false;
                }

                if (pathShuttleThread.isIdle()) {
                    if(pathIdleShuttleMap.containsKey(shuttleProtocol.getShuttleNo())) {
                        Long idleTime = pathIdleShuttleMap.get(shuttleProtocol.getShuttleNo());
                        if((System.currentTimeMillis() - idleTime) > 1000 * 10) {
                            //检测障碍物车
                            boolean checkObstacle = shuttleOperaUtils.checkObstacle(shuttleProtocol.getCurrentLocNo(), new ArrayList<Integer>() {{
                                add(shuttleNo);
                            }}, totalNodeList);
                            pathIdleShuttleMap.remove(shuttleProtocol.getShuttleNo());
                        }
                    }else {
                        pathIdleShuttleMap.put(shuttleProtocol.getShuttleNo(), System.currentTimeMillis());
                    }
                }
                return false;//node has shuttle
            }
        }

        //检测节点是否被使用
        for (TrafficControlDataModel controlDataModel : trafficControlDataList) {
        for (TrafficControlDataModel controlDataModel : allTrafficControlList) {
            List<NavigateNode> list = controlDataModel.getTotalNodeList();
            for (int i = 0; i < list.size(); i++) {
                NavigateNode node = list.get(i);
@@ -529,21 +204,207 @@
        }

        //交管接收
        TrafficControlDataModel model = new TrafficControlDataModel();
        model.setShuttleNo(shuttleNo);
        model.setTaskNo(taskNo);
        model.setNodeList(nodeList);
        model.setTotalNodeList(totalNodeList);
        trafficControlDataList.add(model);
        OperateTrafficControlParam param = new OperateTrafficControlParam();
        param.setDataModel(applyData);
        param.setOperaType("add");
        operateTrafficControl(param);

        applyRecordsMap.remove(shuttleNo);
        redisUtil.set(RedisKeyType.TRAFFIC_CONTROL_SUCCESS_APPLY.key + shuttleNo + "_" + taskNo, 1, 60 * 60);
        News.info("receipt traffic {},{}", shuttleNo, taskNo);
        return true;
    }

    @Override
    public synchronized boolean trafficReport(List<NavigateNode> nodeList, Integer shuttleNo, Integer taskNo) {
    public boolean applyTrafficControl(List<NavigateNode> totalNodeList, List<NavigateNode> nodeList, Integer shuttleNo, Integer taskNo) {
        Set<String> keys = redisUtil.searchKeys(RedisKeyType.TRAFFIC_CONTROL_APPLY.key + shuttleNo + "_");
        if (!keys.isEmpty()) {
            return false;
        }

        TrafficControlDataModel model = new TrafficControlDataModel();
        model.setShuttleNo(shuttleNo);
        model.setTaskNo(taskNo);
        model.setNodeList(nodeList);
        model.setTotalNodeList(totalNodeList);
        redisUtil.set(RedisKeyType.TRAFFIC_CONTROL_APPLY.key + shuttleNo + "_" + System.currentTimeMillis(), model);
        return true;
    }

    @Override
    public boolean trafficReport(List<NavigateNode> nodeList, Integer shuttleNo, Integer taskNo) {
        //检测车子是否存在管制
        List<TrafficControlDataModel> allTrafficControlList = getAllTrafficControl();
        for (int i = 0; i < allTrafficControlList.size(); i++) {
            TrafficControlDataModel controlDataModel = allTrafficControlList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
                if(controlDataModel.getTaskNo().equals(taskNo)) {
                    OperateTrafficControlParam param = new OperateTrafficControlParam();
                    param.setDataModel(controlDataModel);
                    param.setOperaType("report");
                    param.setReportNodeList(nodeList);
                    redisUtil.set(RedisKeyType.TRAFFIC_CONTROL_REPORT_LIST.key + shuttleNo + "_" + System.currentTimeMillis(), param);
                    return true;
                }
            }
        }
        return false;
    }

    @Override
    public synchronized boolean trafficReportError(Integer shuttleNo, Integer taskNo) {
        ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttleNo);
        if (shuttleThread == null) {
            return false;
        }

        if(shuttleReportErrorMap.containsKey(shuttleNo)) {
            Long errorTime = shuttleReportErrorMap.get(shuttleNo);
            if((System.currentTimeMillis() - errorTime) > 1000 * 10) {
                shuttleReportErrorMap.remove(shuttleNo);
                shuttleThread.restartCalcPath();
            }
        }else {
            shuttleReportErrorMap.put(shuttleNo, System.currentTimeMillis());
        }
        return true;
    }

    @Override
    public boolean cancelTrafficControl(Integer shuttleNo, Integer taskNo) {
        TrafficControlDataModel dataModel = queryTrafficControl(shuttleNo, taskNo);
        if (dataModel == null) {
            return false;
        }

        OperateTrafficControlParam param = new OperateTrafficControlParam();
        param.setDataModel(dataModel);
        param.setOperaType("cancel");
        param.setForceCancel(false);
        return operateTrafficControl(param);
    }

    @Override
    public boolean forceCancelTrafficControl(Integer shuttleNo) {
        TrafficControlDataModel dataModel = queryTrafficControl(shuttleNo);
        if (dataModel == null) {
            return false;
        }

        OperateTrafficControlParam param = new OperateTrafficControlParam();
        param.setDataModel(dataModel);
        param.setOperaType("cancel");
        param.setForceCancel(true);
        return operateTrafficControl(param);
    }

    @Override
    public TrafficControlDataModel queryTrafficControl(Integer shuttleNo, Integer taskNo) {
        //检测车子是否存在管制
        List<TrafficControlDataModel> allTrafficControlList = getAllTrafficControl();
        for (int i = 0; i < allTrafficControlList.size(); i++) {
            TrafficControlDataModel controlDataModel = allTrafficControlList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
                if(controlDataModel.getTaskNo().equals(taskNo)) {
                    return controlDataModel;
                }
            }
        }
        return null;
    }

    @Override
    public TrafficControlDataModel queryTrafficControl(Integer shuttleNo) {
        //检测车子是否存在管制
        List<TrafficControlDataModel> allTrafficControlList = getAllTrafficControl();
        for (int i = 0; i < allTrafficControlList.size(); i++) {
            TrafficControlDataModel controlDataModel = allTrafficControlList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
                return controlDataModel;
            }
        }
        return null;
    }

    @Override
    public List<TrafficControlDataModel> getAllTrafficControl() {
        return trafficControlDataListRead;
    }

    @Override
    public synchronized boolean operateTrafficControl(OperateTrafficControlParam param) {
        long startTime = System.currentTimeMillis();
        String operaType = param.getOperaType();
        News.info("Operate Traffic Control is Start " + operaType);

        boolean result = false;
        if (operaType.equals("add")) {
            result = addTrafficControlDataList(param);
        } else if (operaType.equals("cancel")) {
            result = removeTrafficControlDataList(param);
        } else if (operaType.equals("report")) {
            result = reportTrafficControlDataList(param);
        }

        this.trafficControlDataListRead = trafficControlDataList.stream()
                .map(model -> new TrafficControlDataModel(
                        model.getShuttleNo(),
                        model.getTaskNo(),
                        new ArrayList<>(model.getNodeList()),
                        new ArrayList<>(model.getTotalNodeList())
                ))
                .collect(Collectors.toList());
        News.info("Operate Traffic Control is end " + (System.currentTimeMillis() - startTime) + "ms");
        return result;
    }

    public synchronized boolean addTrafficControlDataList(OperateTrafficControlParam param) {
        TrafficControlDataModel dataModel = param.getDataModel();
        trafficControlDataList.add(dataModel);
        return true;
    }

    public synchronized boolean removeTrafficControlDataList(OperateTrafficControlParam param) {
        //检测车子是否存在管制
        TrafficControlDataModel dataModel = param.getDataModel();
        Boolean forceCancel = param.getForceCancel();

        Integer shuttleNo = dataModel.getShuttleNo();
        Integer taskNo = dataModel.getTaskNo();

        int idx = -1;
        for (int i = 0; i < trafficControlDataList.size(); i++) {
            TrafficControlDataModel controlDataModel = trafficControlDataList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
                if (forceCancel) {
                    idx = i;
                    break;
                }else {
                    if(controlDataModel.getTaskNo().equals(taskNo)) {
                        idx = i;
                        break;
                    }
                }
            }
        }

        if(idx == -1) {
            return false;
        }

        trafficControlDataList.remove(idx);//取消管制
        redisUtil.del(RedisKeyType.TRAFFIC_CONTROL_SUCCESS_APPLY.key + shuttleNo + "_" + taskNo);
        return true;
    }

    public synchronized boolean reportTrafficControlDataList(OperateTrafficControlParam param) {
        //检测车子是否存在管制
        TrafficControlDataModel dataModel = param.getDataModel();
        List<NavigateNode> reportNodeList = param.getReportNodeList();

        Integer shuttleNo = dataModel.getShuttleNo();
        Integer taskNo = dataModel.getTaskNo();

        for (int i = 0; i < trafficControlDataList.size(); i++) {
            TrafficControlDataModel controlDataModel = trafficControlDataList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
@@ -552,7 +413,7 @@
                    List<NavigateNode> totalNodeList = controlDataModel.getTotalNodeList();

                    List<String> reportList = new ArrayList<>();
                    for (NavigateNode node : nodeList) {
                    for (NavigateNode node : reportNodeList) {
                        reportList.add(Utils.getLocNo(node.getX(), node.getY(), node.getZ()));
                    }

@@ -566,56 +427,12 @@

                    controlDataModel.setTotalNodeList(newTotalNodeList);
                    trafficControlDataList.set(i, controlDataModel);
                    shuttleReportErrorMap.remove(shuttleNo);
                    return true;
                }
            }
        }
        return false;
    }

    @Override
    public synchronized boolean cancelTrafficControl(Integer shuttleNo, Integer taskNo) {
        //检测车子是否存在管制
        for (int i = 0; i < trafficControlDataList.size(); i++) {
            TrafficControlDataModel controlDataModel = trafficControlDataList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
                if(controlDataModel.getTaskNo().equals(taskNo)) {
                    trafficControlDataList.remove(i);//取消管制
                    return true;
                }
            }
        }
        return false;
    }

    @Override
    public boolean forceCancelTrafficControl(Integer shuttleNo) {
        //检测车子是否存在管制
        for (int i = 0; i < trafficControlDataList.size(); i++) {
            TrafficControlDataModel controlDataModel = trafficControlDataList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
                trafficControlDataList.remove(i);//取消管制
                return true;
            }
        }
        return false;
    }

    @Override
    public TrafficControlDataModel queryTrafficControl(Integer shuttleNo) {
        //检测车子是否存在管制
        for (int i = 0; i < trafficControlDataList.size(); i++) {
            TrafficControlDataModel controlDataModel = trafficControlDataList.get(i);
            if(shuttleNo.equals(controlDataModel.getShuttleNo())) {
                return controlDataModel;
            }
        }
        return null;
    }

    @Override
    public List<TrafficControlDataModel> getAllTrafficControl() {
        return trafficControlDataList;
    }

    @Override