#path similarity

Junjie

2024-12-11 8ef2bb5e46d84594e6ed632c07ea0b47a1bf6c4d

 zy-asrs-wcs/src/main/java/com/zy/asrs/wcs/core/kernel/command/ShuttleCommandService.java

@@ -3,6 +3,7 @@
import com.alibaba.fastjson.JSON;
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import com.zy.asrs.framework.common.Cools;
import com.zy.asrs.framework.common.SpringUtils;
import com.zy.asrs.framework.exception.CoolException;
import com.zy.asrs.wcs.common.ExecuteSupport;
import com.zy.asrs.wcs.core.action.LiftAction;
@@ -24,6 +25,8 @@
import com.zy.asrs.wcs.rcs.thread.DevpThread;
import com.zy.asrs.wcs.rcs.thread.LiftThread;
import com.zy.asrs.wcs.rcs.thread.ShuttleThread;
import com.zy.asrs.wcs.system.entity.Dict;
import com.zy.asrs.wcs.system.service.DictService;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;
@@ -63,6 +66,10 @@
    private BasConveyorStaService basConveyorStaService;
    @Autowired
    private ShuttleDispatcher shuttleDispatcher;
    @Autowired
    private NavigateUtils navigateUtils;
    @Autowired
    private DictService dictService;

    // 计算
    public Boolean accept(Motion motion) {
@@ -125,7 +132,12 @@
                    return true;
                }
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE_LOC_NO;
                shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE;

                if (!checkSimilarityPath(motion, assignCommand)) {
                    return false;
                }

                break;
            case SHUTTLE_TRANSPORT://穿梭车载货行走
                // 如果已经在当前条码则过滤
@@ -136,6 +148,19 @@
                }
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.DFX.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.TRANSPORT;

                if (!checkSimilarityPath(motion, assignCommand)) {
                    return false;
                }

                if (motion.getReleaseLift() == 2) {//执行中释放提升机
                    task.setLiftNo(0);
                    task.setUpdateTime(new Date());
                    if (!taskService.updateById(task)) {
                        return false;
                    }
                }

                break;
            case SHUTTLE_TRANSPORT_TO_CONVEYOR://穿梭车载货进输送线
                BasConveyorSta originStaObj = basConveyorStaService.selectBySiteNo(motion.getDockNo());//获取输送站点
@@ -166,17 +191,32 @@
                        .eq(Loc::getHostId, motion.getHostId())).getCode())) {
                    return true;
                }
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE_LOC_NO;
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.DFX.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.TRANSPORT_TO_CONVEYOR;

                if (!checkSimilarityPath(motion, assignCommand)) {
                    return false;
                }

                break;
            case SHUTTLE_MOVE_LIFT_PALLET://穿梭车顶升并移动
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.DFX.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.PAK_IN;
                shuttleTaskModeType = ShuttleTaskModeType.MOVE_PALLET_LIFT;

                if (!checkSimilarityPath(motion, assignCommand)) {
                    return false;
                }

                shuttleCommands.add(0, shuttleThread.getLiftCommand(motion.getTaskNo(), true));
                break;
            case SHUTTLE_MOVE_DOWN_PALLET://穿梭车移动并托盘下降
                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.DFX.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.PAK_IN;
                shuttleTaskModeType = ShuttleTaskModeType.MOVE_PALLET_DOWN;

                if (!checkSimilarityPath(motion, assignCommand)) {
                    return false;
                }

                shuttleCommands.add(shuttleCommands.size(), shuttleThread.getLiftCommand(motion.getTaskNo(), false));
                break;
            case SHUTTLE_MOVE_FROM_LIFT://出提升机
@@ -216,7 +256,7 @@
//                }

                shuttleCommands = this.shuttleInOutLiftCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE_LOC_NO;
                shuttleTaskModeType = ShuttleTaskModeType.MOVE_OUT_LIFT;
                break;
            case SHUTTLE_MOVE_TO_LIFT://进提升机
                // 判断提升机状态
@@ -252,16 +292,16 @@
//                    return false;
//                }

                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE_LOC_NO;
                shuttleCommands = this.shuttleInOutLiftCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
                shuttleTaskModeType = ShuttleTaskModeType.MOVE_TO_LIFT;
                break;
            case SHUTTLE_CHARGE_ON://充电开
                shuttleTaskModeType = ShuttleTaskModeType.CHARGE;
                shuttleTaskModeType = ShuttleTaskModeType.CHARGE_ON;
                shuttleCommands.add(shuttleThread.getChargeCommand(motion.getTaskNo(), true));
                assignCommand.setCharge(Boolean.TRUE);
                break;
            case SHUTTLE_CHARGE_OFF://充电关
                shuttleTaskModeType = ShuttleTaskModeType.CHARGE;
                shuttleTaskModeType = ShuttleTaskModeType.CHARGE_OFF;
                shuttleCommands.add(shuttleThread.getChargeCommand(motion.getTaskNo(), false));
                assignCommand.setCharge(Boolean.TRUE);
                break;
@@ -278,7 +318,7 @@
                shuttleCommands.add(shuttleThread.getUpdateLocationCommand(motion.getTaskNo(), motion.getTarget()));
                break;
            case SHUTTLE_MOVE_STANDBY://穿梭车移动到待机位
                shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE_LOC_NO;
                shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE;

                //获取全部待机位
                List<String> standbyLocs = JSON.parseArray(motion.getTemp(), String.class);
@@ -510,7 +550,7 @@
        //获取小车移动速度
        Integer runSpeed = Optional.ofNullable(basShuttleService.getOne(new LambdaQueryWrapper<BasShuttle>().eq(BasShuttle::getDeviceId, assignCommand.getDeviceId())).getRunSpeed()).orElse(1000);
        Long hostId = shuttleThread.getDevice().getHostId();
        List<NavigateNode> nodeList = NavigateUtils.calc(startLocNo, endLocNo, mapType, Utils.getShuttlePoints(Integer.parseInt(shuttleThread.getDevice().getDeviceNo()), Utils.getLev(startLocNo)));
        List<NavigateNode> nodeList = navigateUtils.calc(startLocNo, endLocNo, mapType, Utils.getShuttlePoints(Integer.parseInt(shuttleThread.getDevice().getDeviceNo()), Utils.getLev(startLocNo)));
        if (nodeList == null) {
            News.error("{} dash {} can't find navigate path!", startLocNo, endLocNo);
            return null;
@@ -523,7 +563,7 @@

        List<ShuttleCommand> commands = new ArrayList<>();
        //获取分段路径
        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(nodeList);
        ArrayList<ArrayList<NavigateNode>> data = navigateUtils.getSectionPath(nodeList);
        //将每一段路径分成command指令
        for (ArrayList<NavigateNode> nodes : data) {
            //开始路径
@@ -531,7 +571,7 @@

            //目标路径
            NavigateNode endPath = nodes.get(nodes.size() - 1);
            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
            Integer allDistance = navigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
            //通过xy坐标小车二维码
            String startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ(), hostId);
            //通过xy坐标小车二维码
@@ -563,11 +603,10 @@
            return null;//解锁失败
        }


        //获取小车移动速度
        Integer runSpeed = Optional.ofNullable(basShuttleService.getOne(new LambdaQueryWrapper<BasShuttle>().eq(BasShuttle::getDeviceId, assignCommand.getDeviceId())).getRunSpeed()).orElse(1000);
        Long hostId = shuttleThread.getDevice().getHostId();
        List<NavigateNode> nodeList = NavigateUtils.calc(startLocNo, endLocNo, mapType, Utils.getShuttlePoints(Integer.parseInt(shuttleThread.getDevice().getDeviceNo()), Utils.getLev(startLocNo)));
        List<NavigateNode> nodeList = navigateUtils.calc(startLocNo, endLocNo, mapType, Utils.getShuttlePoints(Integer.parseInt(shuttleThread.getDevice().getDeviceNo()), Utils.getLev(startLocNo)));
        if (nodeList == null) {
            News.error("{} dash {} can't find navigate path!", startLocNo, endLocNo);
            return null;
@@ -580,7 +619,7 @@

        List<ShuttleCommand> commands = new ArrayList<>();
        //获取分段路径
        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(nodeList);
        ArrayList<ArrayList<NavigateNode>> data = navigateUtils.getSectionPath(nodeList);
        //将每一段路径分成command指令
        for (ArrayList<NavigateNode> nodes : data) {
            //开始路径
@@ -588,7 +627,7 @@

            //目标路径
            NavigateNode endPath = nodes.get(nodes.size() - 1);
            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
            Integer allDistance = navigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
            //通过xy坐标小车二维码
            String startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ(), hostId);
            //通过xy坐标小车二维码
@@ -604,5 +643,35 @@
        return commands;
    }

    private boolean checkSimilarityPath(Motion motion, ShuttleAssignCommand assignCommand) {
        String movePath = motion.getMovePath();
        if (Cools.isEmpty(movePath)) {
            return false;
        }

        Double similarityRef = 0.9D;
        Dict similarityRefDict = dictService.getOne(new LambdaQueryWrapper<Dict>()
                .eq(Dict::getFlag, "similarityRef")
                .eq(Dict::getStatus, 1));
        if (similarityRefDict != null) {
            similarityRef = Double.parseDouble(similarityRefDict.getValue());
        }

        List<NavigateNode> originPath = JSON.parseArray(movePath, NavigateNode.class);
        List<NavigateNode> finalPath = assignCommand.getNodes();

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

        Double similarity = navigateUtils.similarityPath(originPath, finalPath);
        if (similarity <= similarityRef) {
            News.error("{} dash {} path similarity mismatch!", motion.getOrigin(), motion.getTarget());
            return false;
        }

        return true;
    }


}