diff --git a/servo/src/main/java/com/galaxis/rcs/RCSService.java b/servo/src/main/java/com/galaxis/rcs/RCSService.java index 2e5d267..455eb2d 100644 --- a/servo/src/main/java/com/galaxis/rcs/RCSService.java +++ b/servo/src/main/java/com/galaxis/rcs/RCSService.java @@ -3,7 +3,7 @@ package com.galaxis.rcs; import com.galaxis.rcs.common.entity.AddTaskRequest; import com.galaxis.rcs.common.entity.AddTaskResult; import com.galaxis.rcs.common.enums.LCCDirection; -import com.galaxis.rcs.connector.cl2.Cl2Item; +import com.galaxis.rcs.connector.amr.cl2.Cl2Item; import com.galaxis.rcs.plan.path.PathUtils; import com.google.common.base.Strings; import com.yvan.logisticsModel.LogisticsRuntime; diff --git a/servo/src/main/java/com/galaxis/rcs/amr/AmrAgvItem.java b/servo/src/main/java/com/galaxis/rcs/amr/AmrAgvItem.java new file mode 100644 index 0000000..a7f38f4 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/amr/AmrAgvItem.java @@ -0,0 +1,884 @@ +package com.galaxis.rcs.amr; + +import com.galaxis.rcs.common.entity.RcsTaskPlan; +import com.galaxis.rcs.common.enums.*; +import com.galaxis.rcs.connector.amr.AmrDeviceConnector; +import com.galaxis.rcs.plan.PlanTaskSequence; +import com.galaxis.rcs.plan.path.PathUtils; +import com.galaxis.rcs.amr.receiveEntity.AmrHeartbeatMessage; +import com.galaxis.rcs.amr.receiveEntity.base.CurBatteryData; +import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage; +import com.galaxis.rcs.amr.sendEntity.RcsSRMessage; +import com.galaxis.rcs.amr.sendEntity.RcsSetLocationMessage; +import com.google.common.collect.Queues; +import com.yvan.entity.AgvStatusVo; +import com.yvan.logisticsModel.ExecutorItem; +import com.yvan.logisticsModel.LogisticsRuntime; +import com.yvan.logisticsModel.StaticItem; +import lombok.Getter; +import lombok.SneakyThrows; +import lombok.extern.slf4j.Slf4j; +import org.clever.core.json.JsonWrapper; +import org.clever.data.redis.Redis; +import org.clever.data.redis.RedisAdmin; + +import java.util.*; +import java.util.concurrent.BlockingQueue; +import java.util.concurrent.locks.LockSupport; + +@Slf4j +public abstract class AmrAgvItem extends ExecutorItem { + private static final int BLOCKING_QUEUE_CAPACITY = 100; + private static final Redis redis = RedisAdmin.getRedis(); + + // ip + public volatile String ip; + // agv名称 + public volatile String agvName; + // agv类型 + public volatile String agvType; + // agv型号 + public volatile String agvModel; + // AMR功能型号 + public volatile String agvFnModel; + // 电池信息 + public volatile CurBatteryData battery; + // agv当前x坐标 + public volatile double x; + // agv当前y坐标 + public volatile double y; + // agv当前z坐标 + public volatile double z; + // 当前所在站点的逻辑X坐标 Int32 + public volatile int logicX; + // 当前所在站点的逻辑Y坐标 Int32 + public volatile int logicY; + // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 + public volatile short direction; + // agv当前转动角度值 + public volatile double orientation; + + private volatile boolean isPaused = false; + + private volatile PosDirection lastPausedPosition; + + // 任务模式 + @Getter + private volatile AmrTaskMode __taskMode; + + + // 执行中的任务 + public List runningDeviceTaskList = new ArrayList<>(); + + /** + * 当前执行的任务规划列表 + */ + public volatile PlanTaskSequence planTaskSequence; + + /** + * 当前执行的设备任务列表 + */ + final BlockingQueue deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY); + + final AmrDeviceConnector amrDeviceConnector = new AmrDeviceConnector(this.runtime); + + public final AmrMessageHandler amrMessageHandler; + + /** + * 连接器线程 + */ + public final AmrConnectorThread connectorThread; + + public AmrAgvItem(LogisticsRuntime logisticsRuntime, Map raw) { + super(logisticsRuntime, raw); + this.connectorThread = new AmrConnectorThread(this, this.amrDeviceConnector, logisticsRuntime); + this.amrMessageHandler = logisticsRuntime.amrMessageHandler; + } + + @Override + public boolean isRunning() { + return connectorThread.isRunning(); + } + + public abstract RcsConfigMessage getConfig(); + + + @Override + public void start() { + this.amrMessageHandler.registeHeartBeatSet(this); + + // 查询当前状态 + requestCurrentStatus(); + this.isRunning = true; + + this.startConnector(); + } + + @Override + public void stop() { + // 停止连接器线程 + stopConnector(); + + this.amrMessageHandler.unregisteHeartBeatSet(this); + + // 清理任务序列 + if (planTaskSequence != null) { + planTaskSequence = null; + } + // 清理设备任务队列 + deviceTaskQueue.clear(); + // 清理运行中的设备任务列表 + runningDeviceTaskList.clear(); + // 更新Redis状态 + updateRedisStatus(); + } + + public synchronized void dispatchTask(PlanTaskSequence taskSequence) { + if (!isFree()) { + if (!this.runtime.isRunning()) { + throw new IllegalStateException("runtime is not running!"); + } + if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) { + throw new IllegalStateException("AGV is busy with an existing task sequence"); + } + if (!deviceTaskQueue.isEmpty()) { + throw new IllegalStateException("AGV has pending device tasks in the queue"); + } + if (!isOnline) { + throw new IllegalStateException("AGV is not online and cannot accept new tasks"); + } + + throw new IllegalStateException("AGV is not free to accept new tasks"); + } + + this.planTaskSequence = taskSequence; + buildPlanToDeviceTask(); + this.runtime.eventManager.firePlanTaskSequenceAcceptEvent(this, taskSequence); + connectorThread.resumeProcessing(); + } + + public synchronized void pauseTask() { + if (planTaskSequence == null) { + throw new IllegalStateException("No active task to pause"); + } + + isPaused = true; + lastPausedPosition = new PosDirection(logicX, logicY, PathUtils.getDirectionByArmDirection(direction)); + + // 发送停止指令 + RcsSRMessage stopMsg = new RcsSRMessage(this.runtime); + stopMsg.SeqNo = amrMessageHandler.getNewSeqNo(); + stopMsg.OperationCode = 0; // 停止 + stopMsg.StopX = logicX; + stopMsg.StopY = logicY; + + try { + amrMessageHandler.sendCmdSR(this.getId(), stopMsg); + } catch (Exception e) { + log.error("Failed to send stop command to AGV {}", this.getId(), e); + } + + this.runtime.eventManager.firePlanTaskSequencePauseEvent(this, planTaskSequence); + } + + public synchronized void resumeTask() { + if (!isPaused) { + throw new IllegalStateException("Task is not paused"); + } + + // 检查当前位置是否与暂停位置一致 + if (Math.abs(logicX - lastPausedPosition.logicX()) > 1 || + Math.abs(logicY - lastPausedPosition.logicY()) > 1 || + PathUtils.getDirectionByArmDirection(direction) != lastPausedPosition.direction()) { + // 需要返回暂停位置 + throw new RuntimeException("AGV position has changed since pause, cannot resume task safely"); + } + + isPaused = false; + connectorThread.resumeProcessing(); + this.runtime.eventManager.firePlanTaskSequenceResumeEvent(this, planTaskSequence); + } + + @SneakyThrows + public synchronized void cancelTask() { + // 发送取消指令 + amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo()); + + if (planTaskSequence != null) { + planTaskSequence = null; + } + if (!deviceTaskQueue.isEmpty()) { + deviceTaskQueue.clear(); + } + // todo 取消运行中的设备任务 + runningDeviceTaskList.clear(); + // 唤醒连接器线程 + LockSupport.unpark(connectorThread); + this.runtime.eventManager.firePlanTaskSequenceCancelEvent(this, planTaskSequence); + } + + @SneakyThrows + public void setPositionAndDirection(int x, int y, short direction) { + RcsSetLocationMessage setLoc = new RcsSetLocationMessage(amrMessageHandler.getNewSeqNo()); + setLoc.X = (short) x; + setLoc.Y = (short) y; + setLoc.Direction = direction; + + amrMessageHandler.sendCmdSetLocation(this.getId(), setLoc); + } + + @SneakyThrows + public void requestCurrentStatus() { + amrMessageHandler.sendCmdQueryStatus(this.getId()); + } + + public boolean isFree() { + // return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning()); + if (!this.runtime.isRunning()) { + return false; + } + if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) { + return false; + } + if (!deviceTaskQueue.isEmpty()) { + return false; + } + if (this.isPaused) { + return false; + } + // if (this.taskMode != AmrTaskMode.AMR_FREE_MODE) { + // return false; + // } + return this.isOnline; + } + + public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) { + + updatePosition(logicX, logicY, direction); + // 查找当前分组任务 + for (AmrDeviceTask task : runningDeviceTaskList) { + task.taskGroupStatus = taskStatus; + if (taskStatus == 4) { + if (task.taskStatus != 4) { + this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task); + task.taskStatus = 4; + } + + // 更新计划任务 + List planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList(); + for (RcsTaskPlan planTask : planTaskList) { + if (PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) { + continue; + } + + planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); + this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask); + } + } + } + + if (planTaskSequence != null && planTaskSequence.isAllCompleted()) { + this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence); + this.runningDeviceTaskList.clear(); + planTaskSequence = null; + } + LockSupport.unpark(connectorThread); + } + + public void updatePosition(int logicX, int logicY, short direction) { + int oldX = this.logicX; + int oldY = this.logicY; + short oldDirection = this.direction; + + this.logicX = logicX; + this.logicY = logicY; + this.direction = direction; + + LCCDirection oldLccDirection = PathUtils.getDirectionByArmDirection(oldDirection); + LCCDirection newLccDirection = PathUtils.getDirectionByArmDirection(direction); + + // 更新Redis + updateRedisStatus(); + + // 触发位置变化事件 + if (oldX != logicX || oldY != logicY) { + this.runtime.eventManager.firePosChangedEvent(this, + new PosDirection(logicX, logicY, newLccDirection), + new PosDirection(oldX, oldY, oldLccDirection)); + } + + if (oldDirection != direction) { + this.runtime.eventManager.fireDirectionChangedEvent(this, newLccDirection, oldLccDirection); + } + + boolean needCompute = false; + + // 从 runningDeviceTaskList 里面，找到完成到什么阶段 + // 比如 (1,2) -> (2,2) -> (3,2) , 如果 updatePosition=3,2 ，那么前2个任务都要完成 + int finishTargetIndex = -1; + if (this.runningDeviceTaskList != null && !this.runningDeviceTaskList.isEmpty() && + this.planTaskSequence != null && !this.planTaskSequence.isEmpty()) { + + for (int i = 0; i < runningDeviceTaskList.size(); i++) { + AmrDeviceTask task = runningDeviceTaskList.get(i); + if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.direction == this.direction) { + if (task.taskStatus < 4) { + finishTargetIndex = i; + } + break; + } + } + + if (finishTargetIndex >= 0) { + needCompute = true; + // 标记前面的任务都完成了 + for (int i = 0; i <= finishTargetIndex; i++) { + AmrDeviceTask task = runningDeviceTaskList.get(i); + task.taskStatus = 4; // 标记为完成 + this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task); + + // 更新计划任务 + RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId); + if (planTask != null && !PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) { + planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); + this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask); + } + } + + if (planTaskSequence.isAllCompleted()) { + this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence); + this.runningDeviceTaskList.clear(); + planTaskSequence = null; + } + } + } + + // BannerUtils.printConfig(log, "updatePosition", new String[]{ + // "logicX: " + logicX, + // "logicY: " + logicY, + // "direction: " + direction, + // "finishTargetIndex: " + finishTargetIndex, + // "runningDeviceSize:" + (this.runningDeviceTaskList == null ? "null" : this.runningDeviceTaskList.size()), + // "planTask:" + (this.planTaskSequence == null ? "null" : + // ("\n" + Joiner.on("\n").join((List) this.planTaskSequence.toPrettyMap().get("items"))) + // ) + // }); + + if (needCompute && this.runningDeviceTaskList.size() > 0) { + int index = this.runningDeviceTaskList.size() - 1; + AmrDeviceTask task = this.runningDeviceTaskList.get(index); + if (task.groupEndPoint != task.endPoint) { + LockSupport.unpark(connectorThread); + } + } + } + + /** + * 更新设备任务状态 暂时没有处理任务取消相关的状态 + * + * @param seqNo + * @param x + * @param y + * @param messageStatus + */ + public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) { + // 更新任务状态逻辑 + if (messageStatus < 2) { + return; + } + + // 任务完成逻辑，在地标检查里 + } + + public void updateTaskMode(int taskMode) { + this.setTaskMode(AmrTaskMode.fromValue(taskMode)); + } + + private void setTaskMode(AmrTaskMode taskMode) { + var originalMode = this.__taskMode; + this.__taskMode = taskMode; + this.runtime.eventManager.fireModeChangeEvent(this, taskMode, originalMode); + updateRedisStatus(); + } + + @SneakyThrows + public void updateRedisStatus() { + String statusKey = getRedisKey("status"); + var state = this.getState(); + var statusMap = new JsonWrapper(state).getInnerMap(); + redis.hPutAll(statusKey, statusMap); + } + + public void handleHeartbeat(AmrHeartbeatMessage heartbeat) { + // 更新在线状态 + String aliveKey = getRedisKey("alive"); + redis.vSet(aliveKey, this.runtime.serverId); + redis.kExpire(aliveKey, 5); // 5秒过期 + + // 更新状态信息 + if (this.battery == null) { + this.battery = new CurBatteryData(); + } + this.battery.SOC = heartbeat.Battery; + this.battery.setTemperature(heartbeat.Temperature.Battery); + + // 检查低电量 + if (this.battery.SOC < 20) { + this.runtime.eventManager.fireLowBatteryEvent(this); + } + + updateRedisStatus(); + } + + public void handleOnlineEvent() { + isOnline = true; + this.runtime.eventManager.fireOnlineEvent(this); + requestCurrentStatus(); + } + + public void handleOfflineEvent() { + isOnline = false; + this.runtime.eventManager.fireOfflineEvent(this); + } + + public String getTaskStatus() { + if (planTaskSequence == null) return "IDLE"; + if (isPaused) return "PAUSED"; + return "EXECUTING"; + } + + + /** + * 启动连接器线程 + */ + public void startConnector() { + if (!connectorThread.isRunning()) { + connectorThread.start(); + } + } + + /** + * 停止连接器线程 + */ + public void stopConnector() { + connectorThread.stop(); + } + + private static final int speed = 1000; + + /** + * 添加任务序列到当前执行器 + */ + protected void buildPlanToDeviceTask() { + PlanTaskSequence sequence = this.planTaskSequence; + 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()); + } + StaticItem groupStartPoint = startPoint; + + Set rotationPlanTaskIdSet = new HashSet<>(); + + // 生成移动报文 + List deviceTaskList = new ArrayList<>(); + List> linkStore = null; + // 检查 planList 是不是全都是我的任务 + for (int i = 0; i < sequence.taskList.size(); i++) { + RcsTaskPlan plan = sequence.taskList.get(i); + String endPointId = plan.getTargetId(); + + if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) { + // 获取目标点信息 + StaticItem pointItem = runtime.getStaticItemById(endPointId); + linkStore = (List>) pointItem.dt.get("linkStore"); + int d = -1; + if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) { + d = pointItem.logicY >= startPoint.logicY ? AmrAgvItem.CDirection.db : AmrAgvItem.CDirection.dt; + if ((d > direction && d - AmrAgvItem.CDirection.dl != direction) || (d < direction && d + AmrAgvItem.CDirection.dl != direction)) { + throw new RuntimeException("方向错误"); + } + + } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) { + d = pointItem.logicX >= startPoint.logicX ? AmrAgvItem.CDirection.dr : AmrAgvItem.CDirection.dl; + if ((d > direction && d - AmrAgvItem.CDirection.dl != direction) || (d < direction && d + AmrAgvItem.CDirection.dl != direction)) { + throw new RuntimeException("方向错误"); + } + // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX()); + + } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) { + d = direction; + // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX()); + + } else { + throw new RuntimeException("无法识别的点位关系"); + } + AmrDeviceTask deviceTask = new AmrDeviceTask(); + 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.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = pointItem.logicX; + deviceTask.checkLogicY = pointItem.logicY; + 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 = AmrAgvItem.CDirection.dr; + + } else if (r >= 45 && r < 135) { + direction = AmrAgvItem.CDirection.dt; + + } else if (r >= 135 && r < 225) { + direction = AmrAgvItem.CDirection.dl; + + } else if (r >= 225 && r < 315) { + direction = AmrAgvItem.CDirection.db; + } + rotationPlanTaskIdSet.add(plan.getPlanTaskId()); + + } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) { + + if (deviceTaskList.isEmpty()) { + AmrDeviceTask deviceTask = new AmrDeviceTask(); + deviceTask.x = startPoint.logicX; + deviceTask.y = startPoint.logicY; + deviceTask.speed = speed; + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = startPoint; + deviceTask.bizTaskId = plan.getBizTaskId(); + deviceTask.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = startPoint.logicX; + deviceTask.checkLogicY = startPoint.logicY; + deviceTaskList.add(deviceTask); + linkStore = (List>) startPoint.dt.get("linkStore"); + } + AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = AmrAgvItem.COperationType.transplantLoadAndUnload; + deviceTask.pickMode = AmrAgvItem.CPickMode.load; + deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); + //处理取货高度 + StaticItem storeItem = runtime.getStaticItemById(endPointId); + Map storeItemRaw = storeItem.dt; + if (storeItemRaw.containsKey("bays")) { + List> bays = (List>) storeItemRaw.get("bays"); + Map bay = bays.get(plan.getTargetBay()); + List levelHeight = (List) bay.get("levelHeight"); + deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); + } else { + deviceTask.goodsSlotHeight = 1; + } + if (linkStore != null) { + for (Map store : linkStore) { + if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) { + short d = 0; + switch (store.get("direction").toString()) { + case "up": + d = 1; + break; + case "right": + d = 2; + break; + case "down": + d = 3; + break; + case "left": + d = 0; + break; + } + deviceTask.goodsSlotDirection = d; + } + } + } + + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; + + } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) { + if (deviceTaskList.isEmpty()) { + AmrDeviceTask deviceTask = new AmrDeviceTask(); + deviceTask.x = startPoint.logicX; + deviceTask.y = startPoint.logicY; + deviceTask.speed = speed; + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = startPoint; + deviceTask.bizTaskId = plan.getBizTaskId(); + deviceTask.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = startPoint.logicX; + deviceTask.checkLogicY = startPoint.logicY; + deviceTaskList.add(deviceTask); + linkStore = (List>) startPoint.dt.get("linkStore"); + } + AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = AmrAgvItem.COperationType.transplantLoadAndUnload; + deviceTask.pickMode = AmrAgvItem.CPickMode.unload; + deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); + // 处理卸货高度 + StaticItem storeItem = runtime.getStaticItemById(endPointId); + Map storeItemRaw = storeItem.dt; + if (storeItemRaw.containsKey("bays")) { + List> bays = (List>) storeItemRaw.get("bays"); + Map bay = bays.get(plan.getTargetBay()); + List levelHeight = (List) bay.get("levelHeight"); + deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); + } else { + deviceTask.goodsSlotHeight = 1; + } + if (linkStore != null) { + for (Map store : linkStore) { + if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) { + short d = 0; + switch (store.get("direction").toString()) { + case "up": + d = 3; + break; + case "right": + d = 0; + break; + case "down": + d = 1; + break; + case "left": + d = 2; + break; + } + deviceTask.goodsSlotDirection = d; + } + } + } + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; + + } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) { + if (deviceTaskList.isEmpty()) { + AmrDeviceTask deviceTask = new AmrDeviceTask(); + deviceTask.x = startPoint.logicX; + deviceTask.y = startPoint.logicY; + deviceTask.speed = speed; + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = startPoint; + deviceTask.bizTaskId = plan.getBizTaskId(); + deviceTask.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = startPoint.logicX; + deviceTask.checkLogicY = startPoint.logicY; + deviceTaskList.add(deviceTask); + } + AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = AmrAgvItem.COperationType.charge; + deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); + // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm) + deviceTask.chargeDirection = 2; + deviceTask.chargeLocation = 200; + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; + } + + if (!plan.getExecutorId().equals(this.getId())) { + throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId()); + } + + } + + if (deviceTaskList.isEmpty()) { + AmrDeviceTask deviceTask = new AmrDeviceTask(); + deviceTask.x = startPoint.logicX; + deviceTask.y = startPoint.logicY; + deviceTask.speed = speed; + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = startPoint; + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = startPoint.logicX; + deviceTask.checkLogicY = startPoint.logicY; + deviceTaskList.add(deviceTask); + } + + // 标记任务分组结束 + AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + deviceTask.isGroupEnd = true; + // 最后一个规划任务为旋转时需要添加一个endDirection + if (rotationPlanTaskIdSet.size() > 0) { + deviceTask.operationType = AmrAgvItem.COperationType.move; + deviceTask.pickMode = AmrAgvItem.CPickMode.normal; + deviceTask.endDirection = direction; + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + if (deviceTask.movePlanTaskId == null) { + deviceTask.movePlanTaskId = 0L; + } + rotationPlanTaskIdSet.clear(); + } + + // 反向标记任务组 + int lastIndex = deviceTaskList.size() - 1; + for (int i = deviceTaskList.size() - 1; i >= 0; i--) { + AmrDeviceTask d = deviceTaskList.get(i); + if (d.isGroupEnd) { + lastIndex = i; + } else { + d.operationType = deviceTaskList.get(lastIndex).operationType; + d.pickMode = deviceTaskList.get(lastIndex).pickMode; + d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint; + d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint; + d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight; + d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection; + d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection; + d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation; + } + } + + deviceTaskQueue.addAll(deviceTaskList); + String json = JsonWrapper.toJson(deviceTaskList); + log.info("deviceTaskList: {}", json); + } + + public boolean isSamePosition(PosDirection startPos) { + return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() && + PathUtils.getDirectionByArmDirection(this.direction) == startPos.direction(); + } + + 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; + } + + + /** + * 从 AMR 方向转换为 LCC 方向枚举 + * + * @return + */ + public LCCDirection getLCCDirection() { + return switch (direction) { + case 0 -> LCCDirection.RIGHT; + case 1 -> LCCDirection.DOWN; + case 2 -> LCCDirection.LEFT; + case 3 -> LCCDirection.UP; + default -> null; + }; + } + + public short getAmrDirection(LCCDirection lccDirection) { + return switch (lccDirection) { + case RIGHT -> 0; + case DOWN -> 1; + case LEFT -> 2; + case UP -> 3; + default -> -1; // 未知方向 + }; + } + + private String getRedisKey(String type) { + return String.format("lcc:%s:%s:device:%s:%s", + runtime.projectUuid, runtime.envId, this.getId(), type); + } + + public AgvStatusVo getState() { + var ptr = this; + + var state = new AgvStatusVo(); + state.setId(ptr.id); + state.setType(ptr.getT()); + state.setIsOnline(ptr.isOnline); + state.setIsSystemManaged(ptr.isSystemManaged); + + state.setX(ptr.x); + state.setY(ptr.y); + state.setZ(ptr.z); + state.setLogicX(ptr.logicX); + state.setLogicY(ptr.logicY); + + state.setDirection(PathUtils.getDirectionByArmDirection(ptr.direction)); + state.setOrientation(ptr.orientation); + state.setSoc(ptr.battery == null ? -1 : ptr.battery.SOC); + state.setMode(ptr.get__taskMode()); + state.setTaskStatus(ptr.getTaskStatus()); + + state.setIsBlocked(ptr.isBlocked); + if (ptr.planTaskSequence != null) { + state.setTaskCompleted(ptr.planTaskSequence.completedCount()); + state.setTaskTotalCount(ptr.planTaskSequence.taskTotalCount()); + state.setBizTaskId(ptr.planTaskSequence.bizTask.getBizTaskId()); + state.setBizTaskType(BizTaskType.fromString(ptr.planTaskSequence.bizTask.getBizType())); + state.setBizTaskStatus(BizTaskStatus.fromString(ptr.planTaskSequence.bizTask.getBizTaskStatus())); + state.setBizTaskFrom(ptr.planTaskSequence.bizTask.getTaskFrom()); + state.setBizTaskTo(ptr.planTaskSequence.bizTask.getTaskTo()); + state.setBizLpn(ptr.planTaskSequence.bizTask.getLpn()); + + state.setLoadBasLocationVo(ptr.planTaskSequence.loadBasLocationVo); + state.setUnloadBasLocationVo(ptr.planTaskSequence.unloadBasLocationVo); + } + + return state; + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvConnectorThread.java b/servo/src/main/java/com/galaxis/rcs/amr/AmrConnectorThread.java similarity index 85% rename from servo/src/main/java/com/galaxis/rcs/amr/PtrAgvConnectorThread.java rename to servo/src/main/java/com/galaxis/rcs/amr/AmrConnectorThread.java index df4a0d9..66bde03 100644 --- a/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvConnectorThread.java +++ b/servo/src/main/java/com/galaxis/rcs/amr/AmrConnectorThread.java @@ -1,8 +1,8 @@ package com.galaxis.rcs.amr; import com.fasterxml.jackson.core.JsonProcessingException; -import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage; +import com.galaxis.rcs.connector.amr.AmrDeviceConnector; import com.yvan.logisticsModel.LogisticsRuntime; import lombok.extern.slf4j.Slf4j; import org.eclipse.paho.mqttv5.common.MqttException; @@ -13,21 +13,21 @@ import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.locks.LockSupport; @Slf4j -public class PtrAgvConnectorThread extends Thread { +public class AmrConnectorThread extends Thread { private final AtomicBoolean running = new AtomicBoolean(false); private final AtomicBoolean paused = new AtomicBoolean(false); private final Object pauseLock = new Object(); - private final Cl2DeviceConnector cl2DeviceConnector; - private final PtrAgvItem ptrAgvItem; + public final AmrDeviceConnector amrDeviceConnector; + private final AmrAgvItem ptrAgvItem; private final LogisticsRuntime logisticsRuntime; private volatile int __currentTaskSeqNo = 0; - volatile PtrAgvDeviceTask __currentTask; + volatile AmrDeviceTask __currentTask; - public PtrAgvConnectorThread(PtrAgvItem ptrAgvItem, Cl2DeviceConnector cl2DeviceConnector, LogisticsRuntime logisticsRuntime) { - super("ExecutorConnector-" + ptrAgvItem.getId()); - this.cl2DeviceConnector = cl2DeviceConnector; - this.ptrAgvItem = ptrAgvItem; + public AmrConnectorThread(AmrAgvItem amrAgvItem, AmrDeviceConnector amrDeviceConnector, LogisticsRuntime logisticsRuntime) { + super("ExecutorConnector-" + amrAgvItem.getId()); + this.amrDeviceConnector = amrDeviceConnector; + this.ptrAgvItem = amrAgvItem; this.logisticsRuntime = logisticsRuntime; } @@ -40,11 +40,11 @@ public class PtrAgvConnectorThread extends Thread { float distance = 0; int taskCount = 0; - PtrAgvDeviceTask startTask = null; + AmrDeviceTask startTask = null; RcsTaskMessage taskMessage = null; // 计算中的任务 - List computingTaskList = new ArrayList<>(); + List computingTaskList = new ArrayList<>(); while (running.get()) { if (paused.get()) { synchronized (pauseLock) { @@ -54,7 +54,7 @@ public class PtrAgvConnectorThread extends Thread { } } - PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take(); + AmrDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take(); if (startTask == null) { startTask = currentTask; taskMessage = new RcsTaskMessage(this.logisticsRuntime); @@ -73,7 +73,7 @@ public class PtrAgvConnectorThread extends Thread { computingTaskList.add(currentTask); - PtrAgvDeviceTask nextTask = this.ptrAgvItem.deviceTaskQueue.peek(); + AmrDeviceTask nextTask = this.ptrAgvItem.deviceTaskQueue.peek(); if (currentTask.isGroupEnd || (taskMessage.Link.size() > 0 && nextTask != null && ((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction)) // 下一个任务和开始任务方向不一致 // 单向移动距离大于2m时并且点位数量大于1 如果碰到当前点位与任务结束点位相同且当前任务还有后续，则继续添加步骤（link）错开，避免任务发生歧义 @@ -92,9 +92,9 @@ public class PtrAgvConnectorThread extends Thread { // 发送任务 this.__currentTaskSeqNo = taskMessage.SeqNo; this.__currentTask = currentTask; - cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage); + amrDeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage); this.ptrAgvItem.runningDeviceTaskList.addAll(computingTaskList); - for (PtrAgvDeviceTask task : computingTaskList) { + for (AmrDeviceTask task : computingTaskList) { task.taskStatus = 1; task.taskGroupStatus = 1; } @@ -111,7 +111,7 @@ public class PtrAgvConnectorThread extends Thread { // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒 LockSupport.park(); // 阻塞当前线程 } else { - for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) { + for (AmrDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) { if (task.taskGroupStatus < 4) { LockSupport.park(); // 阻塞当前线程 break; diff --git a/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvDeviceTask.java b/servo/src/main/java/com/galaxis/rcs/amr/AmrDeviceTask.java similarity index 98% rename from servo/src/main/java/com/galaxis/rcs/amr/PtrAgvDeviceTask.java rename to servo/src/main/java/com/galaxis/rcs/amr/AmrDeviceTask.java index d38b946..d994fa3 100644 --- a/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvDeviceTask.java +++ b/servo/src/main/java/com/galaxis/rcs/amr/AmrDeviceTask.java @@ -5,7 +5,7 @@ import com.yvan.logisticsModel.StaticItem; import java.util.HashSet; import java.util.Set; -public class PtrAgvDeviceTask { +public class AmrDeviceTask { //该段目标点X坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 public int x; //该段目标点Y坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 diff --git a/servo/src/main/java/com/galaxis/rcs/amr/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/amr/AmrMessageHandler.java index 5daec1f..d41eb20 100644 --- a/servo/src/main/java/com/galaxis/rcs/amr/AmrMessageHandler.java +++ b/servo/src/main/java/com/galaxis/rcs/amr/AmrMessageHandler.java @@ -517,14 +517,14 @@ public class AmrMessageHandler { /** * 注册心跳单元 */ - public void registeHeartBeatSet(PtrAgvItem ptrAgvItem) { + public void registeHeartBeatSet(AmrAgvItem ptrAgvItem) { this.heartBeatSet.add(ptrAgvItem.getId()); } /** * 注销心跳单元 */ - public void unregisteHeartBeatSet(PtrAgvItem ptrAgvItem) { + public void unregisteHeartBeatSet(AmrAgvItem ptrAgvItem) { this.heartBeatSet.remove(ptrAgvItem.getId()); } diff --git a/servo/src/main/java/com/galaxis/rcs/amr/FM600AgvItem.java b/servo/src/main/java/com/galaxis/rcs/amr/FM600AgvItem.java new file mode 100644 index 0000000..7e570c4 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/amr/FM600AgvItem.java @@ -0,0 +1,18 @@ +package com.galaxis.rcs.amr; + +import com.yvan.logisticsModel.LogisticsRuntime; +import lombok.extern.slf4j.Slf4j; + +import java.util.*; + +/** + * 侧叉式AGV执行器 + */ +@Slf4j +public abstract class FM600AgvItem extends AmrAgvItem { + + public FM600AgvItem(LogisticsRuntime logisticsRuntime, Map raw) { + super(logisticsRuntime, raw); + } + +} diff --git a/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvConnector.java b/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvConnector.java deleted file mode 100644 index 104e90b..0000000 --- a/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvConnector.java +++ /dev/null @@ -1,5 +0,0 @@ -package com.galaxis.rcs.amr; - -public abstract class PtrAgvConnector { - -} diff --git a/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvItem.java b/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvItem.java index 9592317..4b602c2 100644 --- a/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvItem.java +++ b/servo/src/main/java/com/galaxis/rcs/amr/PtrAgvItem.java @@ -1,889 +1,30 @@ package com.galaxis.rcs.amr; -import com.galaxis.rcs.common.entity.RcsTaskPlan; -import com.galaxis.rcs.common.enums.*; -import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; -import com.galaxis.rcs.plan.PlanTaskSequence; -import com.galaxis.rcs.plan.path.PathUtils; -import com.galaxis.rcs.amr.receiveEntity.AmrHeartbeatMessage; -import com.galaxis.rcs.amr.receiveEntity.base.CurBatteryData; import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage; -import com.galaxis.rcs.amr.sendEntity.RcsSRMessage; -import com.galaxis.rcs.amr.sendEntity.RcsSetLocationMessage; -import com.google.common.collect.Queues; -import com.yvan.entity.AgvStatusVo; -import com.yvan.entity.BasLocationVo; -import com.yvan.logisticsModel.ExecutorItem; import com.yvan.logisticsModel.LogisticsRuntime; -import com.yvan.logisticsModel.StaticItem; -import lombok.Getter; -import lombok.SneakyThrows; import lombok.extern.slf4j.Slf4j; -import org.clever.core.Conv; -import org.clever.core.json.JsonWrapper; -import org.clever.data.redis.Redis; -import org.clever.data.redis.RedisAdmin; import java.util.*; -import java.util.concurrent.BlockingQueue; -import java.util.concurrent.locks.LockSupport; /** * 侧叉式AGV执行器 */ @Slf4j -public abstract class PtrAgvItem extends ExecutorItem { - private static final int BLOCKING_QUEUE_CAPACITY = 100; - private static final Redis redis = RedisAdmin.getRedis(); - - // ip - public volatile String ip; - // agv名称 - public volatile String agvName; - // agv类型 - public volatile String agvType; - // agv型号 - public volatile String agvModel; - // AMR功能型号 - public volatile String agvFnModel; - // 电池信息 - public volatile CurBatteryData battery; - // agv当前x坐标 - public volatile double x; - // agv当前y坐标 - public volatile double y; - // agv当前z坐标 - public volatile double z; - // 当前所在站点的逻辑X坐标 Int32 - public volatile int logicX; - // 当前所在站点的逻辑Y坐标 Int32 - public volatile int logicY; - // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 - public volatile short direction; - // agv当前转动角度值 - public volatile double orientation; - - private volatile boolean isPaused = false; - - private volatile PosDirection lastPausedPosition; - - // 任务模式 - @Getter - private volatile AmrTaskMode __taskMode; - - - // 执行中的任务 - public List runningDeviceTaskList = new ArrayList<>(); - - /** - * 当前执行的任务规划列表 - */ - public volatile PlanTaskSequence planTaskSequence; - - /** - * 当前执行的设备任务列表 - */ - final BlockingQueue deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY); - - final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(this.runtime); - - public final AmrMessageHandler amrMessageHandler; - - /** - * 连接器线程 - */ - private final PtrAgvConnectorThread connectorThread; +public class PtrAgvItem extends AmrAgvItem { public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map raw) { super(logisticsRuntime, raw); - this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime); - this.amrMessageHandler = logisticsRuntime.amrMessageHandler; } @Override - public boolean isRunning() { - return connectorThread.isRunning(); - } - - public abstract RcsConfigMessage getConfig(); - - - @Override - public void start() { - this.amrMessageHandler.registeHeartBeatSet(this); - - // 查询当前状态 - requestCurrentStatus(); - this.isRunning = true; - - this.startConnector(); - } - - @Override - public void stop() { - // 停止连接器线程 - stopConnector(); - - this.amrMessageHandler.unregisteHeartBeatSet(this); - - // 清理任务序列 - if (planTaskSequence != null) { - planTaskSequence = null; - } - // 清理设备任务队列 - deviceTaskQueue.clear(); - // 清理运行中的设备任务列表 - runningDeviceTaskList.clear(); - // 更新Redis状态 - updateRedisStatus(); - } - - public synchronized void dispatchTask(PlanTaskSequence taskSequence) { - if (!isFree()) { - if (!this.runtime.isRunning()) { - throw new IllegalStateException("runtime is not running!"); - } - if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) { - throw new IllegalStateException("AGV is busy with an existing task sequence"); - } - if (!deviceTaskQueue.isEmpty()) { - throw new IllegalStateException("AGV has pending device tasks in the queue"); - } - if (!isOnline) { - throw new IllegalStateException("AGV is not online and cannot accept new tasks"); - } - - throw new IllegalStateException("AGV is not free to accept new tasks"); - } - - this.planTaskSequence = taskSequence; - buildPlanToDeviceTask(); - this.runtime.eventManager.firePlanTaskSequenceAcceptEvent(this, taskSequence); - connectorThread.resumeProcessing(); - } - - public synchronized void pauseTask() { - if (planTaskSequence == null) { - throw new IllegalStateException("No active task to pause"); - } - - isPaused = true; - lastPausedPosition = new PosDirection(logicX, logicY, PathUtils.getDirectionByArmDirection(direction)); - - // 发送停止指令 - RcsSRMessage stopMsg = new RcsSRMessage(this.runtime); - stopMsg.SeqNo = amrMessageHandler.getNewSeqNo(); - stopMsg.OperationCode = 0; // 停止 - stopMsg.StopX = logicX; - stopMsg.StopY = logicY; - - try { - amrMessageHandler.sendCmdSR(this.getId(), stopMsg); - } catch (Exception e) { - log.error("Failed to send stop command to AGV {}", this.getId(), e); - } - - this.runtime.eventManager.firePlanTaskSequencePauseEvent(this, planTaskSequence); - } - - public synchronized void resumeTask() { - if (!isPaused) { - throw new IllegalStateException("Task is not paused"); - } - - // 检查当前位置是否与暂停位置一致 - if (Math.abs(logicX - lastPausedPosition.logicX()) > 1 || - Math.abs(logicY - lastPausedPosition.logicY()) > 1 || - PathUtils.getDirectionByArmDirection(direction) != lastPausedPosition.direction()) { - // 需要返回暂停位置 - throw new RuntimeException("AGV position has changed since pause, cannot resume task safely"); - } - - isPaused = false; - connectorThread.resumeProcessing(); - this.runtime.eventManager.firePlanTaskSequenceResumeEvent(this, planTaskSequence); - } - - @SneakyThrows - public synchronized void cancelTask() { - // 发送取消指令 - amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo()); - - if (planTaskSequence != null) { - planTaskSequence = null; - } - if (!deviceTaskQueue.isEmpty()) { - deviceTaskQueue.clear(); - } - // todo 取消运行中的设备任务 - runningDeviceTaskList.clear(); - // 唤醒连接器线程 - LockSupport.unpark(connectorThread); - this.runtime.eventManager.firePlanTaskSequenceCancelEvent(this, planTaskSequence); - } - - @SneakyThrows - public void setPositionAndDirection(int x, int y, short direction) { - RcsSetLocationMessage setLoc = new RcsSetLocationMessage(amrMessageHandler.getNewSeqNo()); - setLoc.X = (short) x; - setLoc.Y = (short) y; - setLoc.Direction = direction; - - amrMessageHandler.sendCmdSetLocation(this.getId(), setLoc); - } - - @SneakyThrows - public void requestCurrentStatus() { - amrMessageHandler.sendCmdQueryStatus(this.getId()); - } - - public boolean isFree() { - // return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning()); - if (!this.runtime.isRunning()) { - return false; - } - if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) { - return false; - } - if (!deviceTaskQueue.isEmpty()) { - return false; - } - if (this.isPaused) { - return false; - } - // if (this.taskMode != AmrTaskMode.AMR_FREE_MODE) { - // return false; - // } - return this.isOnline; - } - - public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) { - - updatePosition(logicX, logicY, direction); - // 查找当前分组任务 - for (PtrAgvDeviceTask task : runningDeviceTaskList) { - task.taskGroupStatus = taskStatus; - if (taskStatus == 4) { - if (task.taskStatus != 4) { - this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task); - task.taskStatus = 4; - } - - // 更新计划任务 - List planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList(); - for (RcsTaskPlan planTask : planTaskList) { - if (PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) { - continue; - } - - planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); - this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask); - } - } - } - - if (planTaskSequence != null && planTaskSequence.isAllCompleted()) { - this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence); - this.runningDeviceTaskList.clear(); - planTaskSequence = null; - } - LockSupport.unpark(connectorThread); - } - - public void updatePosition(int logicX, int logicY, short direction) { - int oldX = this.logicX; - int oldY = this.logicY; - short oldDirection = this.direction; - - this.logicX = logicX; - this.logicY = logicY; - this.direction = direction; - - LCCDirection oldLccDirection = PathUtils.getDirectionByArmDirection(oldDirection); - LCCDirection newLccDirection = PathUtils.getDirectionByArmDirection(direction); - - // 更新Redis - updateRedisStatus(); - - // 触发位置变化事件 - if (oldX != logicX || oldY != logicY) { - this.runtime.eventManager.firePosChangedEvent(this, - new PosDirection(logicX, logicY, newLccDirection), - new PosDirection(oldX, oldY, oldLccDirection)); - } - - if (oldDirection != direction) { - this.runtime.eventManager.fireDirectionChangedEvent(this, newLccDirection, oldLccDirection); - } - - boolean needCompute = false; - - // 从 runningDeviceTaskList 里面，找到完成到什么阶段 - // 比如 (1,2) -> (2,2) -> (3,2) , 如果 updatePosition=3,2 ，那么前2个任务都要完成 - int finishTargetIndex = -1; - if (this.runningDeviceTaskList != null && !this.runningDeviceTaskList.isEmpty() && - this.planTaskSequence != null && !this.planTaskSequence.isEmpty()) { - - for (int i = 0; i < runningDeviceTaskList.size(); i++) { - PtrAgvDeviceTask task = runningDeviceTaskList.get(i); - if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.direction == this.direction) { - if (task.taskStatus < 4) { - finishTargetIndex = i; - } - break; - } - } - - if (finishTargetIndex >= 0) { - needCompute = true; - // 标记前面的任务都完成了 - for (int i = 0; i <= finishTargetIndex; i++) { - PtrAgvDeviceTask task = runningDeviceTaskList.get(i); - task.taskStatus = 4; // 标记为完成 - this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task); - - // 更新计划任务 - RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId); - if (planTask != null && !PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) { - planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); - this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask); - } - } - - if (planTaskSequence.isAllCompleted()) { - this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence); - this.runningDeviceTaskList.clear(); - planTaskSequence = null; - } - } - } - - // BannerUtils.printConfig(log, "updatePosition", new String[]{ - // "logicX: " + logicX, - // "logicY: " + logicY, - // "direction: " + direction, - // "finishTargetIndex: " + finishTargetIndex, - // "runningDeviceSize:" + (this.runningDeviceTaskList == null ? "null" : this.runningDeviceTaskList.size()), - // "planTask:" + (this.planTaskSequence == null ? "null" : - // ("\n" + Joiner.on("\n").join((List) this.planTaskSequence.toPrettyMap().get("items"))) - // ) - // }); - - if (needCompute && this.runningDeviceTaskList.size() > 0) { - int index = this.runningDeviceTaskList.size() - 1; - PtrAgvDeviceTask task = this.runningDeviceTaskList.get(index); - if (task.groupEndPoint != task.endPoint) { - LockSupport.unpark(connectorThread); - } - } - } - - /** - * 更新设备任务状态 暂时没有处理任务取消相关的状态 - * - * @param seqNo - * @param x - * @param y - * @param messageStatus - */ - public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) { - // 更新任务状态逻辑 - if (messageStatus < 2) { - return; - } - - // 任务完成逻辑，在地标检查里 - } - - public void updateTaskMode(int taskMode) { - this.setTaskMode(AmrTaskMode.fromValue(taskMode)); - } - - private void setTaskMode(AmrTaskMode taskMode) { - var originalMode = this.__taskMode; - this.__taskMode = taskMode; - this.runtime.eventManager.fireModeChangeEvent(this, taskMode, originalMode); - updateRedisStatus(); - } - - @SneakyThrows - public void updateRedisStatus() { - String statusKey = getRedisKey("status"); - var state = this.getState(); - var statusMap = new JsonWrapper(state).getInnerMap(); - redis.hPutAll(statusKey, statusMap); - } - - public void handleHeartbeat(AmrHeartbeatMessage heartbeat) { - // 更新在线状态 - String aliveKey = getRedisKey("alive"); - redis.vSet(aliveKey, this.runtime.serverId); - redis.kExpire(aliveKey, 5); // 5秒过期 - - // 更新状态信息 - if (this.battery == null) { - this.battery = new CurBatteryData(); - } - this.battery.SOC = heartbeat.Battery; - this.battery.setTemperature(heartbeat.Temperature.Battery); - - // 检查低电量 - if (this.battery.SOC < 20) { - this.runtime.eventManager.fireLowBatteryEvent(this); - } - - updateRedisStatus(); - } - - public void handleOnlineEvent() { - isOnline = true; - this.runtime.eventManager.fireOnlineEvent(this); - requestCurrentStatus(); - } - - public void handleOfflineEvent() { - isOnline = false; - this.runtime.eventManager.fireOfflineEvent(this); - } - - public String getTaskStatus() { - if (planTaskSequence == null) return "IDLE"; - if (isPaused) return "PAUSED"; - return "EXECUTING"; - } - - - /** - * 启动连接器线程 - */ - public void startConnector() { - if (!connectorThread.isRunning()) { - connectorThread.start(); - } - } - - /** - * 停止连接器线程 - */ - public void stopConnector() { - connectorThread.stop(); - } - - private static final int speed = 1000; - - /** - * 添加任务序列到当前执行器 - */ - protected void buildPlanToDeviceTask() { - PlanTaskSequence sequence = this.planTaskSequence; - 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()); - } - StaticItem groupStartPoint = startPoint; - - Set rotationPlanTaskIdSet = new HashSet<>(); - - // 生成移动报文 - List deviceTaskList = new ArrayList<>(); - List> linkStore = null; - // 检查 planList 是不是全都是我的任务 - for (int i = 0; i < sequence.taskList.size(); i++) { - RcsTaskPlan plan = sequence.taskList.get(i); - String endPointId = plan.getTargetId(); - - if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) { - // 获取目标点信息 - StaticItem pointItem = runtime.getStaticItemById(endPointId); - linkStore = (List>) pointItem.dt.get("linkStore"); - 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 if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) { - d = direction; - // 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.movePlanTaskId = plan.getPlanTaskId(); - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = pointItem.logicX; - deviceTask.checkLogicY = pointItem.logicY; - 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; - } - rotationPlanTaskIdSet.add(plan.getPlanTaskId()); - - } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) { - - if (deviceTaskList.isEmpty()) { - PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); - deviceTask.x = startPoint.logicX; - deviceTask.y = startPoint.logicY; - deviceTask.speed = speed; - deviceTask.direction = direction; - deviceTask.pickMode = 0; - deviceTask.startPoint = startPoint; - deviceTask.endPoint = startPoint; - deviceTask.bizTaskId = plan.getBizTaskId(); - deviceTask.movePlanTaskId = plan.getPlanTaskId(); - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = startPoint.logicX; - deviceTask.checkLogicY = startPoint.logicY; - deviceTaskList.add(deviceTask); - linkStore = (List>) startPoint.dt.get("linkStore"); - } - PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); - deviceTask.operationType = COperationType.transplantLoadAndUnload; - deviceTask.pickMode = CPickMode.load; - deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); - //处理取货高度 - StaticItem storeItem = runtime.getStaticItemById(endPointId); - Map storeItemRaw = storeItem.dt; - if (storeItemRaw.containsKey("bays")) { - List> bays = (List>) storeItemRaw.get("bays"); - Map bay = bays.get(plan.getTargetBay()); - List levelHeight = (List) bay.get("levelHeight"); - deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); - } else { - deviceTask.goodsSlotHeight = 1; - } - if (linkStore != null) { - for (Map store : linkStore) { - if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) { - short d = 0; - switch (store.get("direction").toString()) { - case "up": - d = 1; - break; - case "right": - d = 2; - break; - case "down": - d = 3; - break; - case "left": - d = 0; - break; - } - deviceTask.goodsSlotDirection = d; - } - } - } - - // 标记任务分组结束 - deviceTask.isGroupEnd = true; - deviceTask.groupEndPoint = deviceTask.endPoint; - deviceTask.groupStartPoint = groupStartPoint; - groupStartPoint = deviceTask.endPoint; - - } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) { - if (deviceTaskList.isEmpty()) { - PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); - deviceTask.x = startPoint.logicX; - deviceTask.y = startPoint.logicY; - deviceTask.speed = speed; - deviceTask.direction = direction; - deviceTask.pickMode = 0; - deviceTask.startPoint = startPoint; - deviceTask.endPoint = startPoint; - deviceTask.bizTaskId = plan.getBizTaskId(); - deviceTask.movePlanTaskId = plan.getPlanTaskId(); - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = startPoint.logicX; - deviceTask.checkLogicY = startPoint.logicY; - deviceTaskList.add(deviceTask); - linkStore = (List>) startPoint.dt.get("linkStore"); - } - PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); - deviceTask.operationType = COperationType.transplantLoadAndUnload; - deviceTask.pickMode = CPickMode.unload; - deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); - // 处理卸货高度 - StaticItem storeItem = runtime.getStaticItemById(endPointId); - Map storeItemRaw = storeItem.dt; - if (storeItemRaw.containsKey("bays")) { - List> bays = (List>) storeItemRaw.get("bays"); - Map bay = bays.get(plan.getTargetBay()); - List levelHeight = (List) bay.get("levelHeight"); - deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); - } else { - deviceTask.goodsSlotHeight = 1; - } - if (linkStore != null) { - for (Map store : linkStore) { - if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) { - short d = 0; - switch (store.get("direction").toString()) { - case "up": - d = 3; - break; - case "right": - d = 0; - break; - case "down": - d = 1; - break; - case "left": - d = 2; - break; - } - deviceTask.goodsSlotDirection = d; - } - } - } - // 标记任务分组结束 - deviceTask.isGroupEnd = true; - deviceTask.groupEndPoint = deviceTask.endPoint; - deviceTask.groupStartPoint = groupStartPoint; - groupStartPoint = deviceTask.endPoint; - - } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) { - if (deviceTaskList.isEmpty()) { - PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); - deviceTask.x = startPoint.logicX; - deviceTask.y = startPoint.logicY; - deviceTask.speed = speed; - deviceTask.direction = direction; - deviceTask.pickMode = 0; - deviceTask.startPoint = startPoint; - deviceTask.endPoint = startPoint; - deviceTask.bizTaskId = plan.getBizTaskId(); - deviceTask.movePlanTaskId = plan.getPlanTaskId(); - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = startPoint.logicX; - deviceTask.checkLogicY = startPoint.logicY; - deviceTaskList.add(deviceTask); - } - PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); - deviceTask.operationType = COperationType.charge; - deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); - // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm) - deviceTask.chargeDirection = 2; - deviceTask.chargeLocation = 200; - // 标记任务分组结束 - deviceTask.isGroupEnd = true; - deviceTask.groupEndPoint = deviceTask.endPoint; - deviceTask.groupStartPoint = groupStartPoint; - groupStartPoint = deviceTask.endPoint; - } - - if (!plan.getExecutorId().equals(this.getId())) { - throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId()); - } - - } - - if (deviceTaskList.isEmpty()) { - PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); - deviceTask.x = startPoint.logicX; - deviceTask.y = startPoint.logicY; - deviceTask.speed = speed; - deviceTask.direction = direction; - deviceTask.pickMode = 0; - deviceTask.startPoint = startPoint; - deviceTask.endPoint = startPoint; - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = startPoint.logicX; - deviceTask.checkLogicY = startPoint.logicY; - deviceTaskList.add(deviceTask); - } - - // 标记任务分组结束 - PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); - deviceTask.groupEndPoint = deviceTask.endPoint; - deviceTask.groupStartPoint = groupStartPoint; - deviceTask.isGroupEnd = true; - // 最后一个规划任务为旋转时需要添加一个endDirection - if (rotationPlanTaskIdSet.size() > 0) { - deviceTask.operationType = COperationType.move; - deviceTask.pickMode = CPickMode.normal; - deviceTask.endDirection = direction; - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - if (deviceTask.movePlanTaskId == null) { - deviceTask.movePlanTaskId = 0L; - } - rotationPlanTaskIdSet.clear(); - } - - // 反向标记任务组 - int lastIndex = deviceTaskList.size() - 1; - for (int i = deviceTaskList.size() - 1; i >= 0; i--) { - PtrAgvDeviceTask d = deviceTaskList.get(i); - if (d.isGroupEnd) { - lastIndex = i; - } else { - d.operationType = deviceTaskList.get(lastIndex).operationType; - d.pickMode = deviceTaskList.get(lastIndex).pickMode; - d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint; - d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint; - d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight; - d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection; - d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection; - d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation; - } - } - - deviceTaskQueue.addAll(deviceTaskList); - String json = JsonWrapper.toJson(deviceTaskList); - log.info("deviceTaskList: {}", json); - } - - public boolean isSamePosition(PosDirection startPos) { - return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() && - PathUtils.getDirectionByArmDirection(this.direction) == startPos.direction(); - } - - 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; - } - - - /** - * 从 AMR 方向转换为 LCC 方向枚举 - * - * @return - */ - public LCCDirection getLCCDirection() { - return switch (direction) { - case 0 -> LCCDirection.RIGHT; - case 1 -> LCCDirection.DOWN; - case 2 -> LCCDirection.LEFT; - case 3 -> LCCDirection.UP; - default -> null; - }; - } - - public short getAmrDirection(LCCDirection lccDirection) { - return switch (lccDirection) { - case RIGHT -> 0; - case DOWN -> 1; - case LEFT -> 2; - case UP -> 3; - default -> -1; // 未知方向 - }; - } - - private String getRedisKey(String type) { - return String.format("lcc:%s:%s:device:%s:%s", - runtime.projectUuid, runtime.envId, this.getId(), type); - } - - public AgvStatusVo getState() { - var ptr = this; - - var state = new AgvStatusVo(); - state.setId(ptr.id); - state.setType(ptr.getT()); - state.setIsOnline(ptr.isOnline); - state.setIsSystemManaged(ptr.isSystemManaged); - - state.setX(ptr.x); - state.setY(ptr.y); - state.setZ(ptr.z); - state.setLogicX(ptr.logicX); - state.setLogicY(ptr.logicY); - - state.setDirection(PathUtils.getDirectionByArmDirection(ptr.direction)); - state.setOrientation(ptr.orientation); - state.setSoc(ptr.battery == null ? -1 : ptr.battery.SOC); - state.setMode(ptr.get__taskMode()); - state.setTaskStatus(ptr.getTaskStatus()); - - state.setIsBlocked(ptr.isBlocked); - if (ptr.planTaskSequence != null) { - state.setTaskCompleted(ptr.planTaskSequence.completedCount()); - state.setTaskTotalCount(ptr.planTaskSequence.taskTotalCount()); - state.setBizTaskId(ptr.planTaskSequence.bizTask.getBizTaskId()); - state.setBizTaskType(BizTaskType.fromString(ptr.planTaskSequence.bizTask.getBizType())); - state.setBizTaskStatus(BizTaskStatus.fromString(ptr.planTaskSequence.bizTask.getBizTaskStatus())); - state.setBizTaskFrom(ptr.planTaskSequence.bizTask.getTaskFrom()); - state.setBizTaskTo(ptr.planTaskSequence.bizTask.getTaskTo()); - state.setBizLpn(ptr.planTaskSequence.bizTask.getLpn()); - - state.setLoadBasLocationVo(ptr.planTaskSequence.loadBasLocationVo); - state.setUnloadBasLocationVo(ptr.planTaskSequence.unloadBasLocationVo); - } - - return state; + public RcsConfigMessage getConfig() { + var content = new RcsConfigMessage(this.runtime); + content.SeqNo = this.amrMessageHandler.getNewSeqNo(); + content.XLength = 1000; + content.YLength = 1000; + content.Gap = 1000; + content.HeartBeat = 60; + content.MqRetryTime = 3; + return content; } } diff --git a/servo/src/main/java/com/galaxis/rcs/amr/PtrConnectorThread.java b/servo/src/main/java/com/galaxis/rcs/amr/PtrConnectorThread.java new file mode 100644 index 0000000..34e7c1a --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/amr/PtrConnectorThread.java @@ -0,0 +1,14 @@ +package com.galaxis.rcs.amr; + +import com.galaxis.rcs.connector.amr.PtrDeviceConnector; +import com.yvan.logisticsModel.LogisticsRuntime; +import lombok.extern.slf4j.Slf4j; + +@Slf4j +public class PtrConnectorThread extends AmrConnectorThread { + + public PtrConnectorThread(PtrAgvItem ptrAgvItem, PtrDeviceConnector ptrDeviceConnector, LogisticsRuntime logisticsRuntime) { + super(ptrAgvItem, ptrDeviceConnector, logisticsRuntime); + + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/AmrDeviceConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/AmrDeviceConnector.java new file mode 100644 index 0000000..e8cfb11 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/AmrDeviceConnector.java @@ -0,0 +1,23 @@ +package com.galaxis.rcs.connector.amr; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.galaxis.rcs.amr.AmrMessageHandler; +import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage; +import com.yvan.logisticsModel.LogisticsRuntime; +import lombok.extern.slf4j.Slf4j; +import org.eclipse.paho.mqttv5.common.MqttException; + +@Slf4j +public class AmrDeviceConnector { + public final AmrMessageHandler amrMessageHandler; + public AmrDeviceConnector(LogisticsRuntime runtime) { + this.amrMessageHandler = runtime.amrMessageHandler; + } + public void sendTask(String vehicleId, RcsTaskMessage rcsTaskMessage) throws MqttException, JsonProcessingException { + // var list = Splitter.on("\n").splitToList(JsonWrapper.toJsonPretty(rcsTaskMessage)); + // String[] ar = new String[list.size()]; + // list.toArray(ar); + // BannerUtils.printConfig(log, "Amr 发送报文", ar); + amrMessageHandler.sendCmdTask(vehicleId, rcsTaskMessage); + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/PtrDeviceConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/PtrDeviceConnector.java new file mode 100644 index 0000000..ec9c376 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/PtrDeviceConnector.java @@ -0,0 +1,22 @@ +package com.galaxis.rcs.connector.amr; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.galaxis.rcs.amr.AmrMessageHandler; +import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage; +import com.yvan.logisticsModel.LogisticsRuntime; +import lombok.extern.slf4j.Slf4j; +import org.eclipse.paho.mqttv5.common.MqttException; + +@Slf4j +public class PtrDeviceConnector extends AmrDeviceConnector { + public PtrDeviceConnector(LogisticsRuntime runtime) { + super(runtime); + } + public void sendTask(String vehicleId, RcsTaskMessage rcsTaskMessage) throws MqttException, JsonProcessingException { + // var list = Splitter.on("\n").splitToList(JsonWrapper.toJsonPretty(rcsTaskMessage)); + // String[] ar = new String[list.size()]; + // list.toArray(ar); + // BannerUtils.printConfig(log, "Amr 发送报文", ar); + amrMessageHandler.sendCmdTask(vehicleId, rcsTaskMessage); + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2DeviceConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2DeviceConnector.java new file mode 100644 index 0000000..219ae89 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2DeviceConnector.java @@ -0,0 +1,19 @@ +package com.galaxis.rcs.connector.amr.cl2; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage; +import com.galaxis.rcs.amr.AmrMessageHandler; +import com.galaxis.rcs.connector.amr.AmrDeviceConnector; +import com.yvan.logisticsModel.LogisticsRuntime; +import lombok.extern.slf4j.Slf4j; +import org.eclipse.paho.mqttv5.common.MqttException; + +/** + * CL2 车型报文推送 + */ +@Slf4j +public class Cl2DeviceConnector extends AmrDeviceConnector { + public Cl2DeviceConnector(LogisticsRuntime runtime) { + super(runtime); + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2Item.java similarity index 94% rename from servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java rename to servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2Item.java index 5589844..d95c30c 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2Item.java @@ -1,4 +1,4 @@ -package com.galaxis.rcs.connector.cl2; +package com.galaxis.rcs.connector.amr.cl2; import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage; import com.yvan.logisticsModel.LogisticsRuntime; diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2TaskManger.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2TaskManger.java new file mode 100644 index 0000000..c79f5c0 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2TaskManger.java @@ -0,0 +1,4 @@ +package com.galaxis.rcs.connector.amr.cl2; + +public class Cl2TaskManger { +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/VirtualCl2Connector.java similarity index 65% rename from servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java rename to servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/VirtualCl2Connector.java index a42cdd1..a119dc9 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/VirtualCl2Connector.java @@ -1,4 +1,4 @@ -package com.galaxis.rcs.connector.cl2; +package com.galaxis.rcs.connector.amr.cl2; import lombok.extern.slf4j.Slf4j; diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/clx/ClxConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/clx/ClxConnector.java new file mode 100644 index 0000000..2990623 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/clx/ClxConnector.java @@ -0,0 +1,4 @@ +package com.galaxis.rcs.connector.amr.clx; + +public interface ClxConnector { +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/clx/ClxConnectorImp.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/clx/ClxConnectorImp.java new file mode 100644 index 0000000..033b032 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/clx/ClxConnectorImp.java @@ -0,0 +1,4 @@ +package com.galaxis.rcs.connector.amr.clx; + +public class ClxConnectorImp { +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/clx/VirtualClxConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/clx/VirtualClxConnector.java similarity index 71% rename from servo/src/main/java/com/galaxis/rcs/connector/clx/VirtualClxConnector.java rename to servo/src/main/java/com/galaxis/rcs/connector/amr/clx/VirtualClxConnector.java index cb16c82..b214a12 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/clx/VirtualClxConnector.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/clx/VirtualClxConnector.java @@ -1,4 +1,4 @@ -package com.galaxis.rcs.connector.clx; +package com.galaxis.rcs.connector.amr.clx; /** * @author cwj diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600DeviceConnector.java similarity index 86% rename from servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java rename to servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600DeviceConnector.java index 1e5b49d..7931c89 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600DeviceConnector.java @@ -1,4 +1,4 @@ -package com.galaxis.rcs.connector.cl2; +package com.galaxis.rcs.connector.amr.fm600; import com.fasterxml.jackson.core.JsonProcessingException; import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage; @@ -11,10 +11,10 @@ import org.eclipse.paho.mqttv5.common.MqttException; * CL2 车型报文推送 */ @Slf4j -public class Cl2DeviceConnector { +public class Fm600DeviceConnector { private final AmrMessageHandler amrMessageHandler; - public Cl2DeviceConnector(LogisticsRuntime runtime) { + public Fm600DeviceConnector(LogisticsRuntime runtime) { this.amrMessageHandler = runtime.amrMessageHandler; } diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600Item.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600Item.java new file mode 100644 index 0000000..6bd07ce --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600Item.java @@ -0,0 +1,27 @@ +package com.galaxis.rcs.connector.amr.fm600; + +import com.galaxis.rcs.amr.AmrAgvItem; +import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage; +import com.yvan.logisticsModel.LogisticsRuntime; +import com.galaxis.rcs.amr.PtrAgvItem; + +import java.util.Map; + +public class Fm600Item extends AmrAgvItem { + + public Fm600Item(LogisticsRuntime logisticsRuntime, Map raw) { + super(logisticsRuntime, raw); + } + + @Override + public RcsConfigMessage getConfig() { + var content = new RcsConfigMessage(this.runtime); + content.SeqNo = this.amrMessageHandler.getNewSeqNo(); + content.XLength = 1000; + content.YLength = 1000; + content.Gap = 1000; + content.HeartBeat = 60; + content.MqRetryTime = 3; + return content; + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600TaskManger.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600TaskManger.java new file mode 100644 index 0000000..7f927cf --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600TaskManger.java @@ -0,0 +1,4 @@ +package com.galaxis.rcs.connector.amr.fm600; + +public class Fm600TaskManger { +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/VirtualFm600Connector.java b/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/VirtualFm600Connector.java new file mode 100644 index 0000000..6b8aabd --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/VirtualFm600Connector.java @@ -0,0 +1,8 @@ +package com.galaxis.rcs.connector.amr.fm600; + +import lombok.extern.slf4j.Slf4j; + +@Slf4j +public class VirtualFm600Connector { + +} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2TaskManger.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2TaskManger.java deleted file mode 100644 index 55a1a7f..0000000 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2TaskManger.java +++ /dev/null @@ -1,4 +0,0 @@ -package com.galaxis.rcs.connector.cl2; - -public class Cl2TaskManger { -} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/clx/ClxConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/clx/ClxConnector.java deleted file mode 100644 index 219c044..0000000 --- a/servo/src/main/java/com/galaxis/rcs/connector/clx/ClxConnector.java +++ /dev/null @@ -1,4 +0,0 @@ -package com.galaxis.rcs.connector.clx; - -public interface ClxConnector { -} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/clx/ClxConnectorImp.java b/servo/src/main/java/com/galaxis/rcs/connector/clx/ClxConnectorImp.java deleted file mode 100644 index b71d46a..0000000 --- a/servo/src/main/java/com/galaxis/rcs/connector/clx/ClxConnectorImp.java +++ /dev/null @@ -1,4 +0,0 @@ -package com.galaxis.rcs.connector.clx; - -public class ClxConnectorImp { -} diff --git a/servo/src/main/java/com/yvan/event/AgvEventManager.java b/servo/src/main/java/com/yvan/event/AgvEventManager.java index 8f7bc66..f4d9371 100644 --- a/servo/src/main/java/com/yvan/event/AgvEventManager.java +++ b/servo/src/main/java/com/yvan/event/AgvEventManager.java @@ -5,7 +5,7 @@ import com.galaxis.rcs.common.enums.LCCDirection; import com.galaxis.rcs.plan.PlanTaskSequence; import com.galaxis.rcs.amr.AmrTaskMode; import com.galaxis.rcs.amr.PosDirection; -import com.galaxis.rcs.amr.PtrAgvDeviceTask; +import com.galaxis.rcs.amr.AmrDeviceTask; import com.yvan.logisticsModel.ExecutorItem; import java.util.Set; @@ -78,11 +78,11 @@ public class AgvEventManager { fireEvent(AgvEventType.PLAN_TASK_SEQUENCE_RESUME, sender, taskSequence); } - public void fireDeviceTaskCompleteEvent(ExecutorItem sender, PtrAgvDeviceTask deviceTask) { + public void fireDeviceTaskCompleteEvent(ExecutorItem sender, AmrDeviceTask deviceTask) { fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, sender, deviceTask); } - public void fireDeviceTaskExceptionEvent(ExecutorItem sender, PtrAgvDeviceTask deviceTask, Object exception) { + public void fireDeviceTaskExceptionEvent(ExecutorItem sender, AmrDeviceTask deviceTask, Object exception) { fireEvent(AgvEventType.DEVICE_TASK_EXCEPTION, sender, deviceTask, exception); } diff --git a/servo/src/main/java/com/yvan/event/AgvEventType.java b/servo/src/main/java/com/yvan/event/AgvEventType.java index 220de03..a6eda44 100644 --- a/servo/src/main/java/com/yvan/event/AgvEventType.java +++ b/servo/src/main/java/com/yvan/event/AgvEventType.java @@ -63,12 +63,12 @@ public enum AgvEventType { PLAN_TASK_SEQUENCE_RESUME, /** * 设备任务已完成 - * (ExecutorItem sender, PtrAgvDeviceTask deviceTask) + * (ExecutorItem sender, AmrDeviceTask deviceTask) */ DEVICE_TASK_COMPLETE, /** * 设备任务异常 - * (ExecutorItem sender, PtrAgvDeviceTask deviceTask, Object exception) + * (ExecutorItem sender, AmrDeviceTask deviceTask, Object exception) */ DEVICE_TASK_EXCEPTION, /** diff --git a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java index 89a1024..efb8056 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java +++ b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java @@ -1,8 +1,10 @@ package com.yvan.logisticsModel; +import com.galaxis.rcs.amr.AmrAgvItem; import com.galaxis.rcs.common.entity.RcsTaskPlan; import com.galaxis.rcs.common.enums.PlanTaskType; -import com.galaxis.rcs.connector.cl2.Cl2Item; +import com.galaxis.rcs.connector.amr.cl2.Cl2Item; +import com.galaxis.rcs.connector.amr.fm600.Fm600Item; import com.galaxis.rcs.inv.InvManager; import com.galaxis.rcs.plan.PlanTaskSequence; import com.galaxis.rcs.plan.path.NavigationGraph; @@ -26,7 +28,6 @@ import lombok.extern.slf4j.Slf4j; import org.apache.commons.lang3.time.DateFormatUtils; import org.clever.core.BannerUtils; import org.clever.core.Conv; -import org.clever.core.DateUtils; import org.clever.data.jdbc.DaoFactory; import org.clever.data.jdbc.QueryDSL; import org.clever.data.redis.Redis; @@ -328,10 +329,18 @@ public class LogisticsRuntime { break; case "cl2": + case "cl3": case "clx": - item = new Cl2Item(this, itemObject); + case "cs1": + item = new PtrAgvItem(this, itemObject); this.executorItemMap.put(item.getId(), (ExecutorItem) item); break; + case "fm600": + item = new Fm600Item(this, itemObject); + this.executorItemMap.put(item.getId(), (ExecutorItem) item); + break; + case "cc5": + break; } }