|
|
|
@ -1,15 +1,25 @@ |
|
|
|
package com.yvan.logisticsModel; |
|
|
|
|
|
|
|
import com.galaxis.rcs.common.entity.RcsTaskPlan; |
|
|
|
import com.galaxis.rcs.common.enums.PlanTaskType; |
|
|
|
import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; |
|
|
|
import com.galaxis.rcs.plan.PlanTaskSequence; |
|
|
|
import com.google.common.collect.Queues; |
|
|
|
import lombok.extern.slf4j.Slf4j; |
|
|
|
|
|
|
|
import java.util.ArrayList; |
|
|
|
import java.util.List; |
|
|
|
import java.util.Map; |
|
|
|
import java.util.concurrent.BlockingQueue; |
|
|
|
|
|
|
|
//0.4m/ss // a max 1.2m/s
|
|
|
|
//90 = 3.5s cl2
|
|
|
|
//90 = 5s // cLX
|
|
|
|
@Slf4j |
|
|
|
public class PtrAgvItem extends ExecutorItem { |
|
|
|
private final int BLOCKING_QUEUE_CAPACITY = 100; |
|
|
|
|
|
|
|
private final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(); |
|
|
|
|
|
|
|
// ip
|
|
|
|
public String ip; |
|
|
|
// agv名称
|
|
|
|
@ -53,6 +63,8 @@ public class PtrAgvItem extends ExecutorItem { |
|
|
|
*/ |
|
|
|
final BlockingQueue<RcsTaskPlan> planQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY); |
|
|
|
|
|
|
|
final BlockingQueue<PtrAgvDeviceTask> deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY); |
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
|
* 连接器线程 |
|
|
|
@ -76,6 +88,8 @@ public class PtrAgvItem extends ExecutorItem { |
|
|
|
System.out.println("Connector stopped for executor: " + this.getId()); |
|
|
|
} |
|
|
|
|
|
|
|
private static final int speed = 1000; |
|
|
|
|
|
|
|
/** |
|
|
|
* 添加任务序列到当前执行器 |
|
|
|
*/ |
|
|
|
@ -83,28 +97,146 @@ public class PtrAgvItem extends ExecutorItem { |
|
|
|
if (sequence == null || sequence.taskList.isEmpty()) { |
|
|
|
return; |
|
|
|
} |
|
|
|
LogisticsRuntime runtime = sequence.logisticsRuntime; |
|
|
|
|
|
|
|
|
|
|
|
short direction = this.direction; |
|
|
|
|
|
|
|
// 获取当前设备点位(逻辑点位)
|
|
|
|
StaticItem startPoint = runtime.getStaticItemByLogicXY(this.logicX, this.logicY); |
|
|
|
if (startPoint == null) { |
|
|
|
log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId()); |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// 生成移动报文
|
|
|
|
List<PtrAgvDeviceTask> deviceTaskList = new ArrayList<>(); |
|
|
|
|
|
|
|
// 检查 planList 是不是全都是我的任务
|
|
|
|
for (RcsTaskPlan plan : sequence.taskList) { |
|
|
|
String endPointId = plan.getTargetId(); |
|
|
|
if (plan.getPlanType().equals(PlanTaskType.MOVE.toString())) { |
|
|
|
// 获取目标点信息
|
|
|
|
StaticItem pointItem = runtime.getStaticItemById(endPointId); |
|
|
|
int d = -1; |
|
|
|
if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) { |
|
|
|
d = pointItem.logicY > startPoint.logicY ? CDirection.db : CDirection.dt; |
|
|
|
if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) { |
|
|
|
throw new RuntimeException("方向错误"); |
|
|
|
} |
|
|
|
|
|
|
|
} else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) { |
|
|
|
d = pointItem.logicX > startPoint.logicX ? CDirection.dr : CDirection.dl; |
|
|
|
if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) { |
|
|
|
throw new RuntimeException("方向错误"); |
|
|
|
} |
|
|
|
// distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
|
|
|
|
|
|
|
|
} else { |
|
|
|
throw new RuntimeException("无法识别的点位关系"); |
|
|
|
} |
|
|
|
PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); |
|
|
|
deviceTask.x = pointItem.logicX; |
|
|
|
deviceTask.y = pointItem.logicY; |
|
|
|
deviceTask.speed = d == direction ? (speed) : (-speed); |
|
|
|
deviceTask.direction = direction; |
|
|
|
deviceTask.pickMode = 0; |
|
|
|
deviceTask.startPoint = startPoint; |
|
|
|
deviceTask.endPoint = pointItem; |
|
|
|
deviceTask.bizTaskId = plan.getBizTaskId(); |
|
|
|
deviceTask.planTaskId = plan.getPlanTaskId(); |
|
|
|
deviceTaskList.add(deviceTask); |
|
|
|
// 设置新的起点
|
|
|
|
startPoint = pointItem; |
|
|
|
|
|
|
|
} else if (plan.getPlanType().equals(PlanTaskType.ROTATION.toString())) { |
|
|
|
|
|
|
|
float r = plan.getTargetRotation().floatValue(); |
|
|
|
while (r > 360) { |
|
|
|
r -= 360; |
|
|
|
} |
|
|
|
while (r < 0) { |
|
|
|
r += 360; |
|
|
|
} |
|
|
|
|
|
|
|
if (r >= 315 || r < 45) { |
|
|
|
direction = CDirection.dr; |
|
|
|
|
|
|
|
} else if (r >= 45 && r < 135) { |
|
|
|
direction = CDirection.dt; |
|
|
|
|
|
|
|
} else if (r >= 135 && r < 225) { |
|
|
|
direction = CDirection.dl; |
|
|
|
|
|
|
|
} else if (r >= 225 && r < 315) { |
|
|
|
direction = CDirection.db; |
|
|
|
} |
|
|
|
|
|
|
|
} else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) { |
|
|
|
|
|
|
|
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); |
|
|
|
deviceTask.operationType = COperationType.transplantLoadAndUnload; |
|
|
|
deviceTask.pickMode = CPickMode.load; |
|
|
|
|
|
|
|
} else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) { |
|
|
|
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); |
|
|
|
deviceTask.operationType = COperationType.transplantLoadAndUnload; |
|
|
|
deviceTask.pickMode = CPickMode.unload; |
|
|
|
|
|
|
|
} else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) { |
|
|
|
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); |
|
|
|
deviceTask.operationType = COperationType.charge; |
|
|
|
} |
|
|
|
|
|
|
|
if (!plan.getExecutorId().equals(this.getId())) { |
|
|
|
throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId()); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
LogisticsRuntime runtime = sequence.logisticsRuntime; |
|
|
|
} |
|
|
|
|
|
|
|
planQueue.addAll(sequence.taskList); |
|
|
|
deviceTaskQueue.addAll(deviceTaskList); |
|
|
|
|
|
|
|
// TODO: 开启轮询线程，等待下一个待执行任务
|
|
|
|
} |
|
|
|
|
|
|
|
public boolean isFree() { |
|
|
|
return (this.planQueue.isEmpty() && this.connectorThread.isRunning()); |
|
|
|
return (this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning()); |
|
|
|
} |
|
|
|
|
|
|
|
public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) { |
|
|
|
super(logisticsRuntime, raw); |
|
|
|
this.connectorThread = new PtrAgvConnectorThread(this, logisticsRuntime); |
|
|
|
this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, this, logisticsRuntime); |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
private static class CDirection { |
|
|
|
private static final short dr = 0; |
|
|
|
private static final short db = 1; |
|
|
|
private static final short dl = 2; |
|
|
|
private static final short dt = 3; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
private static class COperationType { |
|
|
|
public static final short move = 0; |
|
|
|
public static final short load = 1; |
|
|
|
public static final short unpick = 2; |
|
|
|
public static final short charge = 3; |
|
|
|
public static final short transplantLoadAndUnload = 4; |
|
|
|
public static final short rollerLoadAndUnload = 5; |
|
|
|
} |
|
|
|
|
|
|
|
private static class CPickMode { |
|
|
|
public static final short normal = 0; |
|
|
|
public static final short load = 1; |
|
|
|
public static final short unload = 2; |
|
|
|
public static final short adjustHeight = 3; |
|
|
|
public static final short adjustHeightToLoad = 5; |
|
|
|
public static final short adjustHeightToUnload = 6; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
