#

Junjie

2024-03-26 47ecbab8adfc9f02bff571c3b608812f7051f652

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

@@ -1,102 +1,108 @@
//package com.zy.asrs.wcs.core.kernel.command;
//
//import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
//import com.zy.asrs.wcs.core.model.command.ShuttleAssignCommand;
//import com.zy.asrs.wcs.core.model.command.ShuttleCommand;
//import com.zy.asrs.wcs.core.model.enums.DeviceCtgType;
//import com.zy.asrs.wcs.core.model.enums.MotionStsType;
//import com.zy.asrs.wcs.core.service.BasShuttleService;
//import com.zy.asrs.wcs.core.service.LocService;
//import com.zy.asrs.wcs.core.service.TaskService;
//import com.zy.asrs.wcs.core.utils.NavigateMapUtils;
//import com.zy.asrs.wcs.core.utils.RedisUtil;
//import com.zy.asrs.wcs.rcs.cache.SlaveConnection;
//import com.zy.asrs.wcs.rcs.entity.Motion;
//import com.zy.asrs.wcs.rcs.model.enums.SlaveType;
//import com.zy.asrs.wcs.rcs.model.protocol.ShuttleProtocol;
//import com.zy.asrs.wcs.rcs.service.MotionService;
//import com.zy.asrs.wcs.rcs.thread.ShuttleThread;
//import lombok.extern.slf4j.Slf4j;
//import org.springframework.beans.factory.annotation.Autowired;
//import org.springframework.stereotype.Service;
//
//import java.util.ArrayList;
//import java.util.List;
//import java.util.Objects;
//import java.util.Optional;
//
///**
// * Created by vincent on 2023/10/23
// */
//@Slf4j
//@Service
//public class ShuttleCommandService {
//
//    public static final Integer SHUTTLE_ADDITION_COMMAND_SPEED = 500;
//
//    @Autowired
//    private RedisUtil redisUtil;
//    @Autowired
//    private MotionService motionService;
//    @Autowired
//    private TaskService taskService;
//    @Autowired
//    private BasShuttleService basShuttleService;
//    @Autowired
//    private LocService locService;
//    @Autowired
//    private NavigateMapUtils navigateMapUtils;
//
//    // 计算
//    public Boolean accept(Motion motion) {
//        Integer deviceNo = Integer.parseInt(motion.getDevice());
//        ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, deviceNo);
//        ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
//        if (null == shuttleProtocol) {
//            return false;
//        }
//        if (!shuttleProtocol.getIdle()) {//设备不空闲
//            return false;
//        }
//        if (!shuttleProtocol.getPakMk()) {
//            return false;
//        }
//        if (motionService.count(new LambdaQueryWrapper<Motion>()
//                .eq(Motion::getDeviceCtg, DeviceCtgType.SHUTTLE.val())
//                .eq(Motion::getDevice, motion.getDevice())
//                .eq(Motion::getMotionSts, MotionStsType.EXECUTING.val())) > 0) {
//            return false;
//        }
//
//        ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
//        assignCommand.setShuttleNo(deviceNo.shortValue());
//        assignCommand.setTaskNo(motion.getWrkNo().shortValue());
//        assignCommand.setSourceLocNo(motion.getOrigin());
//        assignCommand.setLocNo(motion.getTarget());
//
//        List<ShuttleCommand> shuttleCommands = new ArrayList<>();
//        ShuttleTaskModeType shuttleTaskModeType = null;
//
package com.zy.asrs.wcs.core.kernel.command;

import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import com.zy.asrs.framework.common.Cools;
import com.zy.asrs.framework.exception.CoolException;
import com.zy.asrs.wcs.core.entity.Loc;
import com.zy.asrs.wcs.core.model.NavigateNode;
import com.zy.asrs.wcs.core.model.command.ShuttleAssignCommand;
import com.zy.asrs.wcs.core.model.command.ShuttleCommand;
import com.zy.asrs.wcs.core.model.enums.*;
import com.zy.asrs.wcs.core.service.BasShuttleService;
import com.zy.asrs.wcs.core.service.LocService;
import com.zy.asrs.wcs.core.service.TaskService;
import com.zy.asrs.wcs.core.utils.*;
import com.zy.asrs.wcs.rcs.News;
import com.zy.asrs.wcs.rcs.cache.SlaveConnection;
import com.zy.asrs.wcs.rcs.entity.Motion;
import com.zy.asrs.wcs.rcs.model.enums.ShuttleProtocolStatusType;
import com.zy.asrs.wcs.rcs.model.enums.SlaveType;
import com.zy.asrs.wcs.rcs.model.protocol.ShuttleProtocol;
import com.zy.asrs.wcs.rcs.service.MotionService;
import com.zy.asrs.wcs.rcs.thread.ShuttleThread;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

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

/**
 * Created by vincent on 2023/10/23
 */
@Slf4j
@Service
public class ShuttleCommandService {

    public static final Integer SHUTTLE_ADDITION_COMMAND_SPEED = 500;

    @Autowired
    private RedisUtil redisUtil;
    @Autowired
    private MotionService motionService;
    @Autowired
    private TaskService taskService;
    @Autowired
    private BasShuttleService basShuttleService;
    @Autowired
    private LocService locService;
    @Autowired
    private NavigateMapUtils navigateMapUtils;

    // 计算
    public Boolean accept(Motion motion) {
        Integer deviceNo = Integer.parseInt(motion.getDevice());
        ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, deviceNo);
        ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
        if (null == shuttleProtocol) {
            return false;
        }
        if (!shuttleProtocol.getIdle()) {//设备不空闲
            return false;
        }
        if (!shuttleProtocol.getPakMk()) {
            return false;
        }
        if (motionService.count(new LambdaQueryWrapper<Motion>()
                .eq(Motion::getDeviceCtg, DeviceCtgType.SHUTTLE.val())
                .eq(Motion::getDevice, motion.getDevice())
                .eq(Motion::getMotionSts, MotionStsType.EXECUTING.val())) > 0) {
            return false;
        }

        ShuttleAssignCommand assignCommand = new ShuttleAssignCommand();
        assignCommand.setShuttleNo(deviceNo.shortValue());
        assignCommand.setTaskNo(motion.getWrkNo().shortValue());
        assignCommand.setSourceLocNo(motion.getOrigin());
        assignCommand.setLocNo(motion.getTarget());

        List<ShuttleCommand> shuttleCommands = new ArrayList<>();
        ShuttleTaskModeType shuttleTaskModeType = null;

//        SiemensLiftThread liftThread = null;
//        LiftProtocol liftProtocol = null;
//
//        //判断小车状态
//        if (shuttleProtocol.getBusyStatusType().equals(ShuttleStatusType.IDLE)
//                && shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.IDLE)
//                && shuttleProtocol.getTaskNo().intValue() != 0
//        ) {
//            return false;
//        }
//
//        switch (Objects.requireNonNull(MotionCtgType.get(motion.getMotionCtgEl()))){
//            case SHUTTLE_MOVE:
//                // 如果已经在当前条码则过滤
//                if (String.valueOf(shuttleProtocol.getCurrentCode()).equals(locMastService.selectById(motion.getTarget()).getQrCodeValue())) {
//                    return true;
//                }
//                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
//                shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE_LOC_NO;
//                break;

        //判断小车状态
        if (shuttleProtocol.getIdle()
                && shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.IDLE)
                && shuttleProtocol.getTaskNo() != 0
        ) {
            return false;
        }

        switch (Objects.requireNonNull(MotionCtgType.get(motion.getMotionCtgEl()))){
            case SHUTTLE_MOVE:
                // 如果已经在当前条码则过滤
                if (String.valueOf(shuttleProtocol.getCurrentCode()).equals(locService.getOne(new LambdaQueryWrapper<Loc>()
                        .eq(Loc::getLocNo, motion.getTarget())
                        .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;
                break;
//            case SHUTTLE_MOVE_LIFT_PALLET://穿梭车顶升并移动
//                shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.DFX.id, assignCommand, shuttleThread);
//                shuttleTaskModeType = ShuttleTaskModeType.PAK_IN;
@@ -345,76 +351,76 @@
//                shuttleCommands.add(shuttleThread.getChargeSwitchCommand((short) 1));
//                assignCommand.setCharge(Boolean.TRUE);
//                break;
//            default:
//                throw new CoolException(motion.getMotionCtgEl() + "没有指定任务作业流程！！！");
//        }
//
//        if (Cools.isEmpty(shuttleCommands)) {
//            return false;
//        }
//
//        assert null != shuttleTaskModeType;
//        assignCommand.setTaskMode(shuttleTaskModeType.id.shortValue());//入出库模式
//        assignCommand.setCommands(shuttleCommands);
//
//        if (motion.getOrigin() != null && motion.getTarget() != null) {
//            //所使用的路径进行锁定禁用
//            boolean lockResult = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(motion.getTarget()), shuttleProtocol.getShuttleNo().intValue(), assignCommand.getNodes(), true);//所使用的路径进行锁定禁用
//            if (!lockResult) {
//                return false;//锁定失败
//            }
            default:
                throw new CoolException(motion.getMotionCtgEl() + "没有指定任务作业流程！！！");
        }

        if (Cools.isEmpty(shuttleCommands)) {
            return false;
        }

        assert null != shuttleTaskModeType;
        assignCommand.setTaskMode(shuttleTaskModeType.id.shortValue());//入出库模式
        assignCommand.setCommands(shuttleCommands);

        if (motion.getOrigin() != null && motion.getTarget() != null) {
            //所使用的路径进行锁定禁用
            boolean lockResult = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(motion.getTarget()), Integer.parseInt(shuttleProtocol.getShuttleNo()), assignCommand.getNodes(), true);//所使用的路径进行锁定禁用
            if (!lockResult) {
                return false;//锁定失败
            }
//            shuttleThread.assignWork(assignCommand);
//        }else {
        }else {
//            shuttleThread.assignWork(assignCommand);
//        }
//
//        return Boolean.TRUE;
//    }
//
//    public Boolean finish(Motion motion) {
//        Integer deviceNo = Integer.parseInt(motion.getDevice());
//        ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, deviceNo);
//        ShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
//        if (null == shuttleProtocol) {
//            return false;
//        }
//
//        if (shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getTaskNo().intValue() != motion.getWrkNo()) {
//            return false;
//        }
//
//        //充电任务
//        if (Objects.requireNonNull(MotionCtgType.get(motion.getMotionCtgEl())).equals(MotionCtgType.SHUTTLE_CHARGE_ON)) {
//            // 复位穿梭车
//            shuttleProtocol.setTaskNo((short) 0);
//            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
//            shuttleProtocol.setPakMk(true);
//            return true;
//        }
//
//        if (!shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.WAITING)
//            && !shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.CHARGING_WAITING)
//        ) {
//            return false;
//        }
//
//        if (shuttleProtocol.getBusyStatusType().equals(ShuttleStatusType.BUSY)) {
//            return false;
//        }
//
        }

        return Boolean.TRUE;
    }

    public Boolean finish(Motion motion) {
        Integer deviceNo = Integer.parseInt(motion.getDevice());
        ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, deviceNo);
        ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
        if (null == shuttleProtocol) {
            return false;
        }

        if (shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getTaskNo().intValue() != motion.getWrkNo()) {
            return false;
        }

        //充电任务
        if (Objects.requireNonNull(MotionCtgType.get(motion.getMotionCtgEl())).equals(MotionCtgType.SHUTTLE_CHARGE_ON)) {
            // 复位穿梭车
            shuttleProtocol.setTaskNo(0);
            shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
            shuttleProtocol.setPakMk(true);
            return true;
        }

        if (!shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.WAITING)
            && !shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.CHARGING_WAITING)
        ) {
            return false;
        }

        if (!shuttleProtocol.getIdle()) {
            return false;
        }

//        SiemensLiftThread liftThread = null;
//        LiftProtocol liftProtocol = null;
//
//        switch (Objects.requireNonNull(MotionCtgType.get(motion.getMotionCtgEl()))){
//            case SHUTTLE_MOVE:
//            case SHUTTLE_MOVE_LIFT_PALLET:
//            case SHUTTLE_MOVE_DOWN_PALLET:
//            case SHUTTLE_MOVE_TO_CONVEYOR:
//            case SHUTTLE_MOVE_FROM_CONVEYOR:
//                if (!shuttleProtocol.getCurrentLocNo().equals(motion.getTarget())) {
//                    return false;
//                }
//                break;

        switch (Objects.requireNonNull(MotionCtgType.get(motion.getMotionCtgEl()))){
            case SHUTTLE_MOVE:
            case SHUTTLE_MOVE_LIFT_PALLET:
            case SHUTTLE_MOVE_DOWN_PALLET:
            case SHUTTLE_MOVE_TO_CONVEYOR:
            case SHUTTLE_MOVE_FROM_CONVEYOR:
                if (!shuttleProtocol.getCurrentLocNo().equals(motion.getTarget())) {
                    return false;
                }
                break;
//            case SHUTTLE_MOVE_TO_LIFT:
//            case SHUTTLE_MOVE_FROM_LIFT:
//            case SHUTTLE_TRANSPORT_FROM_LIFT:
@@ -451,73 +457,73 @@
//                }
//
//                break;
//            default:
//                break;
//        }
//
//        // 复位穿梭车
//        shuttleProtocol.setTaskNo((short) 0);
//        shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
//        shuttleProtocol.setPakMk(true);
//
//        return true;
//    }
//
//    public synchronized List<ShuttleCommand> shuttleAssignCommand(String startLocNo, String endLocNo, Integer mapType, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
//        //获取小车移动速度
//        Integer runSpeed = Optional.ofNullable(basShuttleService.selectById(assignCommand.getShuttleNo()).getRunSpeed()).orElse(1000);
//
//        List<NavigateNode> nodeList = NavigateUtils.calc(startLocNo, endLocNo, mapType, Utils.getShuttlePoints(shuttleThread.getSlave().getId(), Utils.getLev(startLocNo)));
//        if (nodeList == null) {
//            News.error("{} dash {} can't find navigate path!", startLocNo, endLocNo);
//            return null;
//        }
//        List<NavigateNode> allNode = new ArrayList<>(nodeList);
//
//        List<ShuttleCommand> commands = new ArrayList<>();
//        //获取分段路径
//        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(nodeList);
//        //将每一段路径分成command指令
//        for (ArrayList<NavigateNode> nodes : data) {
//            //开始路径
//            NavigateNode startPath = nodes.get(0);
//
//            //中间路径
//            NavigateNode middlePath = null;
//            //通过xy坐标小车二维码
//            Short middleCodeNum = null;
//            Integer middleToDistDistance = null;//计算中间点到目标点行走距离
//            if (nodes.size() > 10) {//中段码传倒数第三个
//                //中间路径
//                middlePath = nodes.get(nodes.size() - 3);
//                //通过xy坐标小车二维码
//                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
//                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
//            } else if (nodes.size() > 5) {//中段码传倒数第二个
//                //中间路径
//                middlePath = nodes.get(nodes.size() - 2);
//                //通过xy坐标小车二维码
//                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ());
//                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
//            }
//
//            //目标路径
//            NavigateNode endPath = nodes.get(nodes.size() - 1);
//            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
//            //通过xy坐标小车二维码
//            Short startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ());
//            //通过xy坐标小车二维码
//            Short distCodeNum = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ());
//            //获取移动命令
//            ShuttleCommand command = shuttleThread.getMoveCommand(startCodeNum, distCodeNum, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id, middleCodeNum, middleToDistDistance, runSpeed);
//            command.setNodes(nodes);//将行走节点添加到每一步命令中
//            commands.add(command);
//        }
//
//        assignCommand.setNodes(allNode);//当前任务所占用的节点list
//
//        return commands;
//    }
//
//
//}
            default:
                break;
        }

        // 复位穿梭车
        shuttleProtocol.setTaskNo(0);
        shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
        shuttleProtocol.setPakMk(true);

        return true;
    }

    public synchronized List<ShuttleCommand> shuttleAssignCommand(String startLocNo, String endLocNo, Integer mapType, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
        //获取小车移动速度
        Integer runSpeed = Optional.ofNullable(basShuttleService.getById(assignCommand.getShuttleNo()).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)));
        if (nodeList == null) {
            News.error("{} dash {} can't find navigate path!", startLocNo, endLocNo);
            return null;
        }
        List<NavigateNode> allNode = new ArrayList<>(nodeList);

        List<ShuttleCommand> commands = new ArrayList<>();
        //获取分段路径
        ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(nodeList);
        //将每一段路径分成command指令
        for (ArrayList<NavigateNode> nodes : data) {
            //开始路径
            NavigateNode startPath = nodes.get(0);

            //中间路径
            NavigateNode middlePath = null;
            //通过xy坐标小车二维码
            String middleCodeNum = null;
            Integer middleToDistDistance = null;//计算中间点到目标点行走距离
            if (nodes.size() > 10) {//中段码传倒数第三个
                //中间路径
                middlePath = nodes.get(nodes.size() - 3);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ(), hostId);
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            } else if (nodes.size() > 5) {//中段码传倒数第二个
                //中间路径
                middlePath = nodes.get(nodes.size() - 2);
                //通过xy坐标小车二维码
                middleCodeNum = NavigatePositionConvert.xyToPosition(middlePath.getX(), middlePath.getY(), middlePath.getZ(), hostId);
                middleToDistDistance = NavigateUtils.getMiddleToDistDistance(nodes, middlePath);//计算中间点到目标点行走距离
            }

            //目标路径
            NavigateNode endPath = nodes.get(nodes.size() - 1);
            Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
            //通过xy坐标小车二维码
            String startCodeNum = NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), startPath.getZ(), hostId);
            //通过xy坐标小车二维码
            String distCodeNum = NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), endPath.getZ(), hostId);
            //获取移动命令
            ShuttleCommand command = shuttleThread.getMoveCommand(assignCommand.getTaskNo().intValue(), startCodeNum, distCodeNum, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id.intValue(), runSpeed);
            command.setNodes(nodes);//将行走节点添加到每一步命令中
            commands.add(command);
        }

        assignCommand.setNodes(allNode);//当前任务所占用的节点list

        return commands;
    }


}