package com.zy.asrs.wcs.core.kernel.command;
|
|
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.exception.CoolException;
|
import com.zy.asrs.wcs.common.ExecuteSupport;
|
import com.zy.asrs.wcs.core.action.LiftAction;
|
import com.zy.asrs.wcs.core.action.ShuttleAction;
|
import com.zy.asrs.wcs.core.entity.BasShuttle;
|
import com.zy.asrs.wcs.core.entity.Loc;
|
import com.zy.asrs.wcs.core.model.NavigateNode;
|
import com.zy.asrs.wcs.core.model.command.LiftAssignCommand;
|
import com.zy.asrs.wcs.core.model.command.LiftCommand;
|
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.core.entity.Motion;
|
import com.zy.asrs.wcs.rcs.constant.DeviceRedisConstant;
|
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.LiftProtocol;
|
import com.zy.asrs.wcs.rcs.model.protocol.ShuttleProtocol;
|
import com.zy.asrs.wcs.core.service.MotionService;
|
import com.zy.asrs.wcs.rcs.thread.LiftThread;
|
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.*;
|
|
/**
|
* 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;
|
@Autowired
|
private ShuttleAction shuttleAction;
|
@Autowired
|
private LiftAction liftAction;
|
@Autowired
|
private LiftDispatcher liftDispatcher;
|
|
// 计算
|
public Boolean accept(Motion motion) {
|
Integer deviceNo = Integer.parseInt(motion.getDevice());
|
ShuttleThread shuttleThread = (ShuttleThread) SlaveConnection.get(SlaveType.Shuttle, deviceNo);
|
if (shuttleThread == null) {
|
return false;
|
}
|
ShuttleProtocol shuttleProtocol = shuttleThread.getStatus();
|
if (null == shuttleProtocol) {
|
return false;
|
}
|
|
if (!shuttleThread.isIdle(new ExecuteSupport() {
|
@Override
|
public Boolean judgement() {
|
if (Objects.equals(MotionCtgType.get(motion.getMotionCtgEl()), MotionCtgType.SHUTTLE_CHARGE_OFF)) {//非关闭充电motion，需要判断设备状态
|
return false;//不需要判断状态
|
}
|
return true;//需要判断状态
|
}
|
})) {//设备不空闲
|
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);
|
assignCommand.setTaskNo(motion.getTaskNo());
|
assignCommand.setDeviceTaskNo(shuttleThread.generateDeviceTaskNo(motion.getTaskNo(), MotionCtgType.get(motion.getMotionCtgEl())));
|
assignCommand.setSourceLocNo(motion.getOrigin());
|
assignCommand.setLocNo(motion.getTarget());
|
assignCommand.setDeviceId(Integer.parseInt(motion.getDevice()));
|
|
List<ShuttleCommand> shuttleCommands = new ArrayList<>();
|
ShuttleTaskModeType shuttleTaskModeType = null;
|
|
LiftThread liftThread = null;
|
LiftProtocol liftProtocol = null;
|
|
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;
|
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;
|
shuttleCommands.add(shuttleCommands.size(), shuttleThread.getLiftCommand(motion.getTaskNo(), false));
|
break;
|
case SHUTTLE_MOVE_FROM_LIFT://出提升机
|
// 判断提升机状态
|
liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, Integer.parseInt(motion.getTemp()));
|
if (liftThread == null) {
|
return false;
|
}
|
|
liftProtocol = liftThread.getStatus();
|
if (liftProtocol == null) {
|
return false;
|
}
|
|
// 判断提升机是否空闲
|
if (!liftThread.isIdle(MotionCtgType.SHUTTLE_MOVE_FROM_LIFT)) {
|
return false;
|
}
|
|
if (liftProtocol.getLev() != liftDispatcher.getLiftLevOffset(liftThread.getDevice().getId().intValue(), Utils.getLev(motion.getTarget()))) {//判断提升机是否达到目标层
|
return false;
|
}
|
|
//判断提升机是否被锁定
|
if (!liftThread.isLock(new ExecuteSupport() {
|
@Override
|
public Boolean judgement() {
|
return true;
|
}
|
})) {
|
return false;
|
}
|
|
// //判断提升机工作号是否和当前任务相同
|
// if (liftProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
|
// return false;
|
// }
|
|
shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
|
shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE_LOC_NO;
|
break;
|
case SHUTTLE_MOVE_TO_LIFT://进提升机
|
// 判断提升机状态
|
liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, Integer.parseInt(motion.getTemp()));
|
if (liftThread == null) {
|
return false;
|
}
|
liftProtocol = liftThread.getStatus();
|
if (liftProtocol == null) {
|
return false;
|
}
|
// 判断提升机是否空闲
|
if (!liftThread.isIdle(MotionCtgType.SHUTTLE_MOVE_TO_LIFT)) {
|
return false;
|
}
|
|
if (liftProtocol.getLev() != liftDispatcher.getLiftLevOffset(liftThread.getDevice().getId().intValue(), Utils.getLev(motion.getTarget()))) {//判断提升机是否达到目标层
|
return false;
|
}
|
|
//判断提升机是否被锁定
|
if (!liftThread.isLock(new ExecuteSupport() {
|
@Override
|
public Boolean judgement() {
|
return true;
|
}
|
})) {
|
return false;
|
}
|
|
// //判断提升机工作号是否和当前任务相同
|
// if (liftProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
|
// return false;
|
// }
|
|
shuttleCommands = this.shuttleAssignCommand(motion.getOrigin(), motion.getTarget(), NavigationMapType.NORMAL.id, assignCommand, shuttleThread);
|
shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_MOVE_LOC_NO;
|
break;
|
case SHUTTLE_CHARGE_ON://充电开
|
shuttleTaskModeType = ShuttleTaskModeType.CHARGE;
|
shuttleCommands.add(shuttleThread.getChargeCommand(motion.getTaskNo(), true));
|
assignCommand.setCharge(Boolean.TRUE);
|
break;
|
case SHUTTLE_CHARGE_OFF://充电关
|
shuttleTaskModeType = ShuttleTaskModeType.CHARGE;
|
shuttleCommands.add(shuttleThread.getChargeCommand(motion.getTaskNo(), false));
|
assignCommand.setCharge(Boolean.TRUE);
|
break;
|
case SHUTTLE_PALLET_LIFT://托盘顶升
|
shuttleTaskModeType = ShuttleTaskModeType.PALLET_LIFT;
|
shuttleCommands.add(shuttleThread.getLiftCommand(motion.getTaskNo(), true));
|
break;
|
case SHUTTLE_PALLET_DOWN://托盘下降
|
shuttleTaskModeType = ShuttleTaskModeType.PALLET_DOWN;
|
shuttleCommands.add(shuttleThread.getLiftCommand(motion.getTaskNo(), false));
|
break;
|
case SHUTTLE_UPDATE_LOCATION://小车坐标更新
|
shuttleTaskModeType = ShuttleTaskModeType.SHUTTLE_UPDATE_LOCATION;
|
shuttleCommands.add(shuttleThread.getUpdateLocationCommand(motion.getTaskNo(), motion.getTarget()));
|
break;
|
default:
|
throw new CoolException(motion.getMotionCtgEl() + "没有指定任务作业流程！！！");
|
}
|
|
if (Cools.isEmpty(shuttleCommands)) {
|
return false;
|
}
|
|
assert null != shuttleTaskModeType;
|
assignCommand.setTaskMode(shuttleTaskModeType.id);//入出库模式
|
assignCommand.setCommands(shuttleCommands);
|
|
if (motion.getOrigin() != null && motion.getTarget() != null) {
|
//所使用的路径进行锁定禁用
|
boolean lockResult = navigateMapUtils.writeNavigateNodeToRedisMap(Utils.getLev(motion.getTarget()), shuttleProtocol.getShuttleNo(), assignCommand.getNodes(), true);//所使用的路径进行锁定禁用
|
if (!lockResult) {
|
return false;//锁定失败
|
}
|
}
|
|
boolean result = shuttleAction.assignWork(shuttleThread.getDevice(), assignCommand);
|
return result;
|
}
|
|
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.getTaskNo()) {
|
return false;
|
}
|
|
//充电任务
|
if (Objects.requireNonNull(MotionCtgType.get(motion.getMotionCtgEl())).equals(MotionCtgType.SHUTTLE_CHARGE_ON)) {
|
if (shuttleProtocol.getHasCharge() || shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.CHARGING_WAITING)) {
|
// 复位穿梭车
|
shuttleThread.setSyncTaskNo(0);
|
shuttleThread.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
|
shuttleThread.setPakMk(true);
|
return true;
|
}
|
}
|
|
if (!shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.WAITING)
|
&& !shuttleProtocol.getProtocolStatusType().equals(ShuttleProtocolStatusType.CHARGING_WAITING)
|
) {
|
return false;
|
}
|
|
//判断设备是否空闲
|
if (!shuttleThread.isIdle()) {
|
return false;
|
}
|
|
LiftThread 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;
|
case SHUTTLE_MOVE_TO_LIFT:
|
liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, Integer.parseInt(motion.getTemp()));
|
if (liftThread == null) {
|
return false;
|
}
|
liftProtocol = liftThread.getStatus();
|
|
if (!shuttleProtocol.getCurrentLocNo().equals(motion.getTarget())) {
|
return false;
|
}
|
|
//判断提升机是否被锁定
|
if (liftThread.isLock(new ExecuteSupport() {
|
@Override
|
public Boolean judgement() {
|
return false;
|
}
|
})) {
|
//解锁提升机
|
List<LiftCommand> lockCommand = liftThread.getLockCommand(motion.getTaskNo(), false);//获取提升机解锁命令
|
|
LiftAssignCommand liftAssignCommand = new LiftAssignCommand();
|
liftAssignCommand.setLiftNo(liftThread.getStatus().getLiftNo());
|
liftAssignCommand.setTaskNo(motion.getTaskNo());
|
ArrayList<LiftCommand> list = new ArrayList<>();
|
list.addAll(lockCommand);
|
liftAssignCommand.setCommands(list);
|
|
liftAction.assignWork(liftThread.getDevice(), liftAssignCommand);
|
return false;//等待下一次轮询
|
}
|
|
//判断小车是否已到位
|
if (liftProtocol.getHasCar()) {
|
List<LiftCommand> signalCommand = liftThread.getShuttleSignalCommand(motion.getTaskNo(), true);//获取小车已到位命令
|
if(signalCommand != null) {
|
boolean hasKey = redisUtil.hasKey(DeviceRedisConstant.COMMAND_TMP + motion.getTaskNo());
|
if (!hasKey) {
|
LiftAssignCommand liftAssignCommand = new LiftAssignCommand();
|
liftAssignCommand.setLiftNo(liftThread.getStatus().getLiftNo());
|
liftAssignCommand.setTaskNo(motion.getTaskNo());
|
ArrayList<LiftCommand> list = new ArrayList<>();
|
list.addAll(signalCommand);
|
liftAssignCommand.setCommands(list);
|
liftAction.assignWork(liftThread.getDevice(), liftAssignCommand);
|
redisUtil.set(DeviceRedisConstant.COMMAND_TMP + motion.getTaskNo(), true, 120);
|
return false;
|
}
|
}
|
}
|
|
//判断提升机工作号是否和当前任务相同
|
if (liftProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
|
return false;
|
}
|
|
if (liftProtocol.getTaskNo() != 0) {
|
//清空提升机号
|
liftThread.setSyncTaskNo(0);
|
}
|
break;
|
case SHUTTLE_MOVE_FROM_LIFT:
|
liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, Integer.parseInt(motion.getTemp()));
|
if (liftThread == null) {
|
return false;
|
}
|
liftProtocol = liftThread.getStatus();
|
|
if (!shuttleProtocol.getCurrentLocNo().equals(motion.getTarget())) {
|
return false;
|
}
|
|
//判断提升机是否被锁定
|
if (liftThread.isLock(new ExecuteSupport() {
|
@Override
|
public Boolean judgement() {
|
return false;
|
}
|
})) {
|
//解锁提升机
|
List<LiftCommand> lockCommand = liftThread.getLockCommand(motion.getTaskNo(), false);//获取提升机解锁命令
|
|
LiftAssignCommand liftAssignCommand = new LiftAssignCommand();
|
liftAssignCommand.setLiftNo(liftThread.getStatus().getLiftNo());
|
liftAssignCommand.setTaskNo(motion.getTaskNo());
|
ArrayList<LiftCommand> list = new ArrayList<>();
|
list.addAll(lockCommand);
|
liftAssignCommand.setCommands(list);
|
|
liftAction.assignWork(liftThread.getDevice(), liftAssignCommand);
|
return false;//等待下一次轮询
|
}
|
|
//判断小车是否已离开
|
if (!liftProtocol.getHasCar()) {
|
List<LiftCommand> signalCommand = liftThread.getShuttleSignalCommand(motion.getTaskNo(), false);//获取小车已驾离命令
|
if(signalCommand != null) {
|
boolean hasKey = redisUtil.hasKey(DeviceRedisConstant.COMMAND_TMP + motion.getTaskNo());
|
if (!hasKey) {
|
redisUtil.set(DeviceRedisConstant.COMMAND_TMP + motion.getTaskNo(), JSON.toJSONString(signalCommand), 120);
|
LiftAssignCommand liftAssignCommand = new LiftAssignCommand();
|
liftAssignCommand.setLiftNo(liftThread.getStatus().getLiftNo());
|
liftAssignCommand.setTaskNo(motion.getTaskNo());
|
ArrayList<LiftCommand> list = new ArrayList<>();
|
list.addAll(signalCommand);
|
liftAssignCommand.setCommands(list);
|
liftAction.assignWork(liftThread.getDevice(), liftAssignCommand);
|
return false;
|
}
|
}
|
}
|
|
//判断提升机工作号是否和当前任务相同
|
if (liftProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
|
return false;
|
}
|
|
if (liftProtocol.getTaskNo() != 0) {
|
//清空提升机号
|
liftThread.setSyncTaskNo(0);
|
}
|
break;
|
case SHUTTLE_TRANSPORT_FROM_LIFT:
|
case SHUTTLE_TRANSPORT_TO_LIFT:
|
case SHUTTLE_MOVE_FROM_LIFT_TO_CONVEYOR:
|
liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, Integer.parseInt(motion.getTemp()));
|
if (liftThread == null) {
|
return false;
|
}
|
liftProtocol = liftThread.getStatus();
|
|
if (!shuttleProtocol.getCurrentLocNo().equals(motion.getTarget())) {
|
return false;
|
}
|
|
//判断提升机是否被锁定
|
if (liftThread.isLock(new ExecuteSupport() {
|
@Override
|
public Boolean judgement() {
|
return false;
|
}
|
})) {
|
//解锁提升机
|
List<LiftCommand> lockCommand = liftThread.getLockCommand(motion.getTaskNo(), false);//获取提升机解锁命令
|
|
LiftAssignCommand liftAssignCommand = new LiftAssignCommand();
|
liftAssignCommand.setLiftNo(liftThread.getStatus().getLiftNo());
|
liftAssignCommand.setTaskNo(motion.getTaskNo());
|
ArrayList<LiftCommand> list = new ArrayList<>();
|
list.addAll(lockCommand);
|
liftAssignCommand.setCommands(list);
|
|
liftAction.assignWork(liftThread.getDevice(), liftAssignCommand);
|
return false;//等待下一次轮询
|
}
|
|
//判断提升机工作号是否和当前任务相同
|
if (liftProtocol.getTaskNo().intValue() != motion.getTaskNo()) {
|
return false;
|
}
|
|
if (liftProtocol.getTaskNo() != 0) {
|
//清空提升机号
|
liftThread.setSyncTaskNo(0);
|
}
|
break;
|
case SHUTTLE_UPDATE_LOCATION://小车坐标更新
|
break;
|
default:
|
break;
|
}
|
|
// 复位穿梭车
|
shuttleThread.setSyncTaskNo(0);
|
shuttleThread.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
|
shuttleThread.setPakMk(true);
|
|
return true;
|
}
|
|
public synchronized List<ShuttleCommand> shuttleAssignCommand(String startLocNo, String endLocNo, Integer mapType, ShuttleAssignCommand assignCommand, ShuttleThread shuttleThread) {
|
//获取小车移动速度
|
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)));
|
if (nodeList == null) {
|
News.error("{} dash {} can't find navigate path!", startLocNo, endLocNo);
|
return null;
|
}
|
|
List<NavigateNode> allNode = new ArrayList<>();
|
for (NavigateNode node : nodeList) {
|
allNode.add(node.clone());
|
}
|
|
List<ShuttleCommand> commands = new ArrayList<>();
|
//获取分段路径
|
ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(nodeList);
|
//将每一段路径分成command指令
|
for (ArrayList<NavigateNode> nodes : data) {
|
//开始路径
|
NavigateNode startPath = nodes.get(0);
|
|
//目标路径
|
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.getDeviceTaskNo(), startCodeNum, distCodeNum, allDistance, ShuttleRunDirection.get(startPath.getDirection()).id.intValue(), runSpeed, nodes);
|
command.setNodes(nodes);//将行走节点添加到每一步命令中
|
commands.add(command);
|
}
|
|
assignCommand.setNodes(allNode);//当前任务所占用的节点list
|
|
return commands;
|
}
|
|
|
}
|
