#调度避障算法

Junjie

7 天以前 78478aecdc7b125e16cae01954f3a0ad25644b17

#调度避障算法

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

@@ -179,6 +179,15 @@
                return false;
            }

            //检测障碍物车
            boolean checkObstacle = shuttleOperaUtils.checkObstacle(wrkMast.getLocNo(), new ArrayList<Integer>() {{
                add(shuttleProtocol.getShuttleNo());
            }});
            if (checkObstacle) {
                News.info("{}任务，避障范围有小车，等待障碍小车调离中", wrkMast.getWrkNo());
                return false;
            }

            ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
            assignCommand.setShuttleNo(shuttleProtocol.getShuttleNo()); // 四向穿梭车编号
            assignCommand.setTaskMode(ShuttleTaskModeType.TRANSPORT.id);//小车移库任务
@@ -351,6 +360,15 @@
                //小车不在输送站点位置
                shuttleDispatchUtils.dispatchShuttle(wrkMast.getWrkNo(), wrkMast.getSourceLocNo(), wrkMast.getShuttleNo());//调度小车到货物所在库位进行取货
                News.taskInfo(wrkMast.getWrkNo(), "{}任务，{}小车，未到达输送站点，系统等待中", wrkMast.getWrkNo(), shuttleProtocol.getShuttleNo());
                return false;
            }

            //检测障碍物车
            boolean checkObstacle = shuttleOperaUtils.checkObstacle(liftSta.getLocNo(), new ArrayList<Integer>() {{
                add(shuttleProtocol.getShuttleNo());
            }});
            if (checkObstacle) {
                News.info("{}任务，避障范围有小车，等待障碍小车调离中", wrkMast.getWrkNo());
                return false;
            }

@@ -1361,6 +1379,15 @@
                return false;
            }

            //检测障碍物车
            boolean checkObstacle = shuttleOperaUtils.checkObstacle(basShuttleCharge.getWaitLocNo(), new ArrayList<Integer>() {{
                add(shuttleProtocol.getShuttleNo());
            }});
            if (checkObstacle) {
                News.info("{}任务，避障范围有小车，等待障碍小车调离中", wrkMast.getWrkNo());
                return false;
            }

            //调度小车去待机位
            boolean dispatched = shuttleDispatchUtils.dispatchShuttle(wrkMast.getWrkNo(), basShuttleCharge.getWaitLocNo(), wrkMast.getShuttleNo());
            if (!dispatched) {
@@ -1744,6 +1771,15 @@
                return false;//路径计算失败
            }

            //检测障碍物车
            boolean checkObstacle = shuttleOperaUtils.checkObstacle(wrkMast.getLocNo(), new ArrayList<Integer>() {{
                add(shuttleProtocol.getShuttleNo());
            }});
            if (checkObstacle) {
                News.info("{}任务，避障范围有小车，等待障碍小车调离中", wrkMast.getWrkNo());
                return false;
            }

            assignCommand.setCommands(commands);

            wrkMast.setWrkSts(WrkStsType.MOVE_SHUTTLE.sts);//小车移动到目标库位中  309.小车迁出提升机完成 ==> 310.小车移动中

 src/main/java/com/zy/common/utils/ShuttleOperaUtils.java

@@ -1,22 +1,31 @@
package com.zy.common.utils;

import com.baomidou.mybatisplus.mapper.EntityWrapper;
import com.core.common.SpringUtils;
import com.core.exception.CoolException;
import com.zy.asrs.entity.BasShuttle;
import com.zy.asrs.service.BasShuttleService;
import com.zy.asrs.utils.Utils;
import com.zy.common.model.NavigateNode;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.core.News;
import com.zy.core.cache.SlaveConnection;
import com.zy.core.dispatcher.ShuttleDispatchUtils;
import com.zy.core.enums.MapNodeType;
import com.zy.core.enums.ShuttleRunDirection;
import com.zy.core.enums.SlaveType;
import com.zy.core.model.ShuttleSlave;
import com.zy.core.model.command.ShuttleAssignCommand;
import com.zy.core.model.command.ShuttleCommand;
import com.zy.core.model.protocol.ShuttleProtocol;
import com.zy.core.properties.SlaveProperties;
import com.zy.core.thread.ShuttleThread;
import com.zy.system.entity.Config;
import com.zy.system.service.ConfigService;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Component;

import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import java.util.*;

/**
 * 四向车操作工具类
@@ -31,6 +40,14 @@
    private NavigateUtils navigateUtils;
    @Autowired
    private NavigateMapUtils navigateMapUtils;
    @Autowired
    private NavigateMapData navigateMapData;
    @Autowired
    private ConfigService configService;
    @Autowired
    private SlaveProperties slaveProperties;
    @Autowired
    private ShuttleDispatchUtils shuttleDispatchUtils;

    public synchronized List<ShuttleCommand> getStartToTargetCommands(String startLocNo, String endLocNo, Integer mapType, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
        return getStartToTargetCommands(startLocNo, endLocNo, mapType, null, assignCommand, shuttleThread);
@@ -189,6 +206,269 @@
        return commands;
    }

    //检测障碍物车
    public synchronized boolean checkObstacle(String locNo, List<Integer> whiteShuttles) {
        int innerCircle = 0;
        int outerCircle = 3;
        Config avoidInnerCircleConfig = configService.selectOne(new EntityWrapper<Config>().eq("code", "avoidInnerCircle"));
        if(avoidInnerCircleConfig != null) {
            innerCircle = Integer.parseInt(avoidInnerCircleConfig.getValue());
        }

        Config avoidOuterCircleConfig = configService.selectOne(new EntityWrapper<Config>().eq("code", "avoidOuterCircle"));
        if(avoidOuterCircleConfig != null) {
            outerCircle = Integer.parseInt(avoidOuterCircleConfig.getValue());
        }

        ArrayList<int[]> whiteShuttlePointList = new ArrayList<>();
        for (Integer shuttle : whiteShuttles) {
            //获取四向穿梭车线程
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttle);
            if (shuttleThread == null) {
                continue;
            }
            ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
            if (shuttleProtocol == null) {
                continue;
            }
            String currentLocNo = shuttleProtocol.getCurrentLocNo();
            int[] shuttleArr = NavigatePositionConvert.positionToXY(currentLocNo);
            whiteShuttlePointList.add(shuttleArr);
        }

        //获取内圈节点
        List<NavigateNode> innerNodes = getInnerNodes(locNo, innerCircle, whiteShuttlePointList);
        List<Integer> nodesCar = findNodesCar(innerNodes);
        if (nodesCar.isEmpty()) {
            return false;//内圈中无车
        }

        //获取外圈节点
        List<NavigateNode> outerNodes = getOuterNodes(locNo, outerCircle, whiteShuttlePointList, innerNodes);

        //将内圈节点中障碍小车调离
        for (Integer shuttleNo : nodesCar) {
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttleNo);
            if (shuttleThread == null) {
                continue;
            }
            ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
            if (shuttleProtocol == null) {
                continue;
            }
            if (!shuttleThread.isIdle()) {
                continue;
            }

            int nextInt = new Random().nextInt(outerNodes.size());
            NavigateNode randomNode = outerNodes.get(nextInt);
            String randomLocNo = NavigatePositionConvert.nodeToLocNo(randomNode);
            shuttleDispatchUtils.dispatchShuttle(null, randomLocNo, shuttleNo);
        }

        return true;//内圈中有车
    }

    //搜索节点内的小车编号
    private List<Integer> findNodesCar(List<NavigateNode> nodes) {
        List<Integer> list = new ArrayList<>();

        HashMap<String, Integer> carMap = new HashMap<>();
        for (ShuttleSlave slave : slaveProperties.getShuttle()) {
            ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, slave.getId());
            if (shuttleThread == null) {
                continue;
            }
            ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
            if (shuttleProtocol == null) {
                continue;
            }
            String currentLocNo = shuttleProtocol.getCurrentLocNo();
            if (currentLocNo == null) {
                continue;
            }

            carMap.put(currentLocNo, slave.getId());
        }

        for (NavigateNode node : nodes) {
            String locNo = NavigatePositionConvert.nodeToLocNo(node);
            if (carMap.containsKey(locNo)) {
                list.add(carMap.get(locNo));
            }
        }

        return list;
    }

    private List<NavigateNode> getOuterNodes(String locNo, int outerCircle, List<int[]> whiteShuttlePointList, List<NavigateNode> innerNodes) {
        List<NavigateNode> outerNodes = new ArrayList<>();
        List<NavigateNode> outerNodesTmp = new ArrayList<>();
        int lev = Utils.getLev(locNo);
        int[] pointArr = NavigatePositionConvert.positionToXY(locNo);
        NavigateNode currentNode = new NavigateNode(pointArr[0], pointArr[1]);

        int[][] map = navigateMapData.getDataFromRedis(lev, NavigationMapType.NORMAL.id, null, whiteShuttlePointList);
        int nodeValue = map[pointArr[0]][pointArr[1]];
        currentNode.setNodeValue(nodeValue);

        for (int i = 0; i < outerCircle; i++) {
            int idx = i + 1;
            List<NavigateNode> list = extend_nodes(map, currentNode, idx);
            if (list.isEmpty()) {
                continue;
            }
            outerNodesTmp.addAll(list);
        }

        for (NavigateNode node : outerNodesTmp) {
            boolean flag = false;
            for (NavigateNode innerNode : innerNodes) {
                if(node.getX() == innerNode.getX() && node.getY() == innerNode.getY()) {
                    flag = true;
                    break;
                }
            }

            if(flag) {
                continue;
            }

            outerNodes.add(node);
        }

        return outerNodes;
    }

    private List<NavigateNode> getInnerNodes(String locNo, int innerCircle, List<int[]> whiteShuttlePointList) {
        List<NavigateNode> innerNodes = new ArrayList<>();
        int lev = Utils.getLev(locNo);
        int[] pointArr = NavigatePositionConvert.positionToXY(locNo);
        NavigateNode currentNode = new NavigateNode(pointArr[0], pointArr[1]);
        currentNode.setZ(lev);
        innerNodes.add(currentNode);

        int[][] map = navigateMapData.getDataFromRedis(lev, NavigationMapType.NORMAL.id, null, whiteShuttlePointList);
        int nodeValue = map[pointArr[0]][pointArr[1]];
        currentNode.setNodeValue(nodeValue);

        for (int i = 0; i < innerCircle; i++) {
            int idx = i + 1;
            List<NavigateNode> list = extend_nodes(map, currentNode, idx);
            if (list.isEmpty()) {
                continue;
            }
            innerNodes.addAll(list);
        }

        return innerNodes;
    }

    private List<NavigateNode> extend_nodes(int[][] map, NavigateNode startNode, int innerCircleIdx) {
        //默认地图母轨方向x
        String mapDirection = "x";
        ConfigService configService = SpringUtils.getBean(ConfigService.class);
        if (configService != null) {
            Config config = configService.selectOne(new EntityWrapper<Config>()
                    .eq("code", "direction_map")
                    .eq("status", 1));
            if (config != null) {
                mapDirection = config.getValue();
            }
        }

        ArrayList<NavigateNode> nodes = new ArrayList<>();
        int x = startNode.getX();
        int y = startNode.getY();
        int z = startNode.getZ();

        if (mapDirection.equals("x")) {//母轨x方向
            if (map[x][y] == MapNodeType.MAIN_PATH.id) {
                //母轨才能进行上下移动
                if (is_valid(map, x + innerCircleIdx, y)) {
                    NavigateNode node = new NavigateNode(x + innerCircleIdx, y);
                    node.setNodeValue(map[x + innerCircleIdx][y]);
                    node.setZ(z);
                    nodes.add(node);
                }
                if (is_valid(map, x - innerCircleIdx, y)) {
                    NavigateNode node = new NavigateNode(x - innerCircleIdx, y);
                    node.setNodeValue(map[x - innerCircleIdx][y]);
                    node.setZ(z);
                    nodes.add(node);
                }
            }

            if (map[x][y] == MapNodeType.NORMAL_PATH.id || map[x][y] == MapNodeType.MAIN_PATH.id || map[x][y] == MapNodeType.CONVEYOR_CAR_GO.id || map[x][y] == MapNodeType.CHARGE.id || map[x][y] == MapNodeType.LIFT.id) {
                //子轨和母轨、小车可走输送线、充电桩、提升机才能进行左右移动
                if (is_valid(map, x, y + innerCircleIdx)) {
                    NavigateNode node = new NavigateNode(x, y + innerCircleIdx);
                    node.setNodeValue(map[x][y + innerCircleIdx]);
                    node.setZ(z);
                    nodes.add(node);
                }

                if (is_valid(map, x, y - innerCircleIdx)) {
                    NavigateNode node = new NavigateNode(x, y - innerCircleIdx);
                    node.setNodeValue(map[x][y - innerCircleIdx]);
                    node.setZ(z);
                    nodes.add(node);
                }
            }
        }else if (mapDirection.equals("y")) {//母轨y方向
            if (map[x][y] == MapNodeType.MAIN_PATH.id) {
                //母轨才能进行左右移动
                if (is_valid(map, x, y + innerCircleIdx)) {
                    NavigateNode node = new NavigateNode(x, y + innerCircleIdx);
                    node.setNodeValue(map[x][y + innerCircleIdx]);
                    node.setZ(z);
                    nodes.add(node);
                }

                if (is_valid(map, x, y - innerCircleIdx)) {
                    NavigateNode node = new NavigateNode(x, y - innerCircleIdx);
                    node.setNodeValue(map[x][y - innerCircleIdx]);
                    node.setZ(z);
                    nodes.add(node);
                }
            }

            if (map[x][y] == MapNodeType.NORMAL_PATH.id || map[x][y] == MapNodeType.MAIN_PATH.id || map[x][y] == MapNodeType.CONVEYOR_CAR_GO.id || map[x][y] == MapNodeType.CHARGE.id || map[x][y] == MapNodeType.LIFT.id) {
                //子轨和母轨、小车可走输送线、充电桩、提升机才能进行上下移动
                if (is_valid(map, x + innerCircleIdx, y)) {
                    NavigateNode node = new NavigateNode(x + innerCircleIdx, y);
                    node.setNodeValue(map[x + innerCircleIdx][y]);
                    node.setZ(z);
                    nodes.add(node);
                }
                if (is_valid(map, x - innerCircleIdx, y)) {
                    NavigateNode node = new NavigateNode(x - innerCircleIdx, y);
                    node.setNodeValue(map[x - innerCircleIdx][y]);
                    node.setZ(z);
                    nodes.add(node);
                }
            }
        }else {
            throw new CoolException("异常地图方向");
        }

        return nodes;
    }

    private boolean is_valid(int[][] map, int x, int y) {
        if (x < 0 || x >= map.length
                || y < 0 || y >= map[0].length) {
            return false;
        }
        // 如果结点的位置小于0，则不合法
        if (map[x][y] < 0) {
            return false;
        }

        //以上情况都没有则合法
        return true;
    }

//    private boolean checkSimilarityPath(Motion motion, ShuttleAssignCommand assignCommand) {
//        String movePath = motion.getMovePath();
//        if (Cools.isEmpty(movePath)) {
@@ -235,170 +515,6 @@
//        }
//
//        return true;
//    }

//    /**
//     * 计算并获取小车从起点——中间点——目标点之间搬运货物动作命令
//     * @param shuttleNo 小车号
//     * @param wrkNo 工作号
//     * @param startPoint 起点（小车当前位置）
//     * @param targetPoint 目标点（货物目标位置）
//     */
//    public static synchronized ShuttleOperaResult getShuttleTransportCommands(Integer shuttleNo, Integer wrkNo, String startPoint, String targetPoint) {
//        //行走路径
//        ArrayList<NavigateNode> nodes = new ArrayList<>();
//        //命令集合
//        ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
//
//        //计算起点到目标点命令
//        ShuttleOperaResult result = getStartToTargetCommands(shuttleNo, wrkNo, startPoint, targetPoint, NavigationMapType.DFX.id);
//        if (result == null) {
//            //计算结果必须不为空，否则计算失败
//            return null;
//        }
//        nodes.addAll(result.getNodes());
//        //起点位置下发一条顶升命令将货物进行搬运
//        commands.add(NyHttpUtils.getPalletLiftCommand(shuttleNo, wrkNo, true));
//        commands.addAll(result.getCommands());//起点到目标点移动命令
//        //当小车行走到目标点后，需要下发一条下降命令将货物放置
//        commands.add(NyHttpUtils.getPalletLiftCommand(shuttleNo, wrkNo, false));
//        return result(commands, nodes);
//    }
//
//    /**
//     * 计算并获取小车从起点——中间点——目标点之间搬运货物动作命令
//     * @param shuttleNo 小车号
//     * @param wrkNo 工作号
//     * @param startPoint 起点（小车当前位置）
//     * @param middlePoint 中间点（货物位置）
//     * @param targetPoint 目标点（货物目标位置）
//     */
//    public static synchronized ShuttleOperaResult getShuttleTransportCommands(Integer shuttleNo, Integer wrkNo, String startPoint, String middlePoint, String targetPoint) {
//        //行走路径
//        ArrayList<NavigateNode> nodes = new ArrayList<>();
//        //命令集合
//        ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
//
//        if (!startPoint.equals(middlePoint)) {//起点和中间点不一致，需要计算起点到中间点路径
//            //计算起点到中间点命令
//            ShuttleOperaResult result1 = getStartToTargetCommands(shuttleNo, wrkNo, startPoint, middlePoint, NavigationMapType.NORMAL.id);
//            if (result1 == null) {
//                //计算结果必须不为空，否则计算失败
//                return null;
//            }
//            nodes.addAll(result1.getNodes());
//            commands.addAll(result1.getCommands());
//        }
//
//        //计算中间点到目标点命令
//        ShuttleOperaResult result2 = getStartToTargetCommands(shuttleNo, wrkNo, middlePoint, targetPoint, NavigationMapType.DFX.id);
//        if (result2 == null) {
//            //计算结果必须不为空，否则计算失败
//            return null;
//        }
//        nodes.addAll(result2.getNodes());
//        //当小车行走到中间点后，需要下发一条顶升命令将货物进行搬运
//        commands.add(NyHttpUtils.getPalletLiftCommand(shuttleNo, wrkNo, true));
//        commands.addAll(result2.getCommands());//中间点到目标点移动命令
//        //当小车行走到目标点后，需要下发一条下降命令将货物放置
//        commands.add(NyHttpUtils.getPalletLiftCommand(shuttleNo, wrkNo, false));
//        return result(commands, nodes);
//    }
//
//    /**
//     * 获取起点到目标点行走命令
//     */
//    public synchronized ShuttleOperaResult getStartToTargetCommands(Integer shuttleNo, Integer wrkNo, String startPoint, String targetPoint, Integer mapType) {
//        NavigateMapUtils navigateMapUtils = SpringUtils.getBean(NavigateMapUtils.class);
//        //计算起点到目标点行走节点
//        List<NavigateNode> calc = NavigateUtils.calc(startPoint, targetPoint, mapType, Utils.getShuttlePoints(shuttleNo, Utils.getLev(startPoint)), null);
//        if (calc == null) {
//            return null;
//        }
//
//        //命令集合
//        ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
//        List<NavigateNode> allNode = new ArrayList<>();
//
//        //获取分段路径
//        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
//        //将每一段路径分成command指令
//        for (ArrayList<NavigateNode> nodes : data) {
//            //开始路径
//            NavigateNode startPath = nodes.get(0);
//            //目标路径
//            NavigateNode targetPath = nodes.get(nodes.size() - 1);
//            //获取移动命令
//            NyShuttleHttpCommand moveCommand = NyHttpUtils.getMoveCommand(shuttleNo, wrkNo, startPath, targetPath);
//            moveCommand.setNodes(nodes);//将行走节点添加到每一步命令中
//            commands.add(moveCommand);
//
//            allNode.addAll(nodes);
//        }
//
//        boolean result = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(startPoint), shuttleNo, allNode, true);//锁定路径
//        if (!result) {
//            return null;//路径锁定失败
//        }
//        return result(commands, calc);
//    }
//
//    /**
//     * 获取起点到目标点行走命令(可传白名单)
//     */
//    public static synchronized ShuttleOperaResult getStartToTargetCommandsByWhites(Integer shuttleNo, Integer wrkNo, String startPoint, String targetPoint, Integer mapType, List<int[]> whites) {
//        NavigateMapUtils navigateMapUtils = SpringUtils.getBean(NavigateMapUtils.class);
//        //计算起点到目标点行走节点
//        List<NavigateNode> calc = NavigateUtils.calc(startPoint, targetPoint, mapType, Utils.getShuttlePoints(shuttleNo, Utils.getLev(startPoint)), whites);
//        if (calc == null) {
//            return null;
//        }
//
//        //命令集合
//        ArrayList<NyShuttleHttpCommand> commands = new ArrayList<>();
//        List<NavigateNode> allNode = new ArrayList<>();
//
//        //获取分段路径
//        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
//        //将每一段路径分成command指令
//        for (ArrayList<NavigateNode> nodes : data) {
//            //开始路径
//            NavigateNode startPath = nodes.get(0);
//            //目标路径
//            NavigateNode targetPath = nodes.get(nodes.size() - 1);
//            //获取移动命令
//            NyShuttleHttpCommand moveCommand = NyHttpUtils.getMoveCommand(shuttleNo, wrkNo, startPath, targetPath);
//            moveCommand.setNodes(nodes);//将行走节点添加到每一步命令中
//            commands.add(moveCommand);
//
//            allNode.addAll(nodes);
//        }
//
//        //锁定路径时剔除白名单节点
//        ArrayList<NavigateNode> nodes = new ArrayList<>();
//        for (NavigateNode node : allNode) {
//            boolean flag = false;
//            for (int[] white : whites) {
//                if (node.getX() == white[0] && node.getY() == white[1]) {
//                    flag = true;//存在白名单节点
//                    break;//跳过白名单节点
//                }
//            }
//
//            if (!flag) {
//                nodes.add(node);
//            }
//        }
//        boolean result = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(startPoint), shuttleNo, nodes, true);//锁定路径
//        if (!result) {
//            return null;//路径锁定失败
//        }
//        return result(commands, calc);
//    }
//
//    //返回结果集
//    public static ShuttleOperaResult result(List<NyShuttleHttpCommand> commands, List<NavigateNode> nodes) {
//        return new ShuttleOperaResult(commands, nodes);
//    }

}