diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java index 7d1611a..8cd3005 100644 --- a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java +++ b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java @@ -140,6 +140,7 @@ public class AmrMessageHandler { switch (ArmMessageType.fromValue(id)) { case AMR_TASK_COMPLETED: AmrMessage taskCompletedMessage = JacksonUtils.parse(json, typeRef20010Message); + handleTaskCompletedMessage(agvItem, taskCompletedMessage.content); break; case AMR_TASK_STATUS: handleTaskStatusMessage(agvItem, jw, json); @@ -419,6 +420,11 @@ public class AmrMessageHandler { updateRedisNetDelay(agvItem.getId(), netDelay); } + + private void handleTaskCompletedMessage(PtrAgvItem agvItem, AmrTaskCompletedMessage message) { + agvItem.updateTask(message.CurX, message.CurY, message.CurDirection, 4); + } + private void handleLandmarkMessage(PtrAgvItem agvItem, AmrLandmarkMessage message) { // 这是源逻辑，CurLogicX / CurLogicY / CurDirection 需要到 PtrAgvItem 中更新, 因为要触发事件 agvItem.x = message.X; @@ -488,6 +494,7 @@ public class AmrMessageHandler { // agvItem.logicY = completedMessage.Info.CurLogicY; // // agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation; // agvItem.direction = taskCompleted.content.Info.CurDirection; + agvItem.updateTask(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection, 4); agvItem.updatePosition(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection); break; case 8: diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java b/servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java index b0061fc..a606a0f 100644 --- a/servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java +++ b/servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java @@ -11,10 +11,10 @@ import com.galaxis.rcs.ptr.receiveEntity.base.SummaryData; @JsonIgnoreProperties(ignoreUnknown = true) public class AmrTaskCompletedMessage extends AmrCommonMessage { - // 当前X坐标 Double 当前实际位置在地图坐标系中的X坐标 - public double CurX; + // 当前X坐标 UInt16 + public int CurX; // 当前Y坐标 Double 当前实际位置在地图坐标系中的Y坐标 - public double CurY; + public int CurY; // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 public short CurDirection; // 当前方向 Double 角度 diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java b/servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java index 181debb..3236f86 100644 --- a/servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java +++ b/servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java @@ -40,6 +40,9 @@ public class RcsTaskMessage { public Integer GoodsSlotHeight; // 目标货位朝向 UInt8 朝向定义与充电桩朝向相同。 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 public Short GoodsSlotDirection; + // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 + public Short EndDirection; + // 多机构^[1]^的拣货模式 UInt8[3] 数组形式，意义同"PickMode" public List MPickMode; // 多机构^[1]^的目标货位高度 UInt16[3] 单位：mm diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java index e017bff..97dd691 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java @@ -10,6 +10,7 @@ import org.eclipse.paho.mqttv5.common.MqttException; import java.util.ArrayList; import java.util.List; import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.locks.LockSupport; @Slf4j public class PtrAgvConnectorThread extends Thread { @@ -40,8 +41,6 @@ public class PtrAgvConnectorThread extends Thread { int taskCount = 0; PtrAgvDeviceTask startTask = null; - PtrAgvDeviceTask currentTask = null; - PtrAgvDeviceTask nextTask = null; RcsTaskMessage taskMessage = null; // 计算中的任务 @@ -55,26 +54,10 @@ public class PtrAgvConnectorThread extends Thread { } } - // for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) { - // if (task.taskGroupStatus < 3) { - // LockSupport.park(); // 阻塞当前线程 - // break; - // } - // } - - // 从队列中获取任务，如果队列为空则阻塞等待 - if (nextTask == null) { - nextTask = this.ptrAgvItem.deviceTaskQueue.take(); - - } else { - currentTask = nextTask; - } - - if (startTask == null && !nextTask.isLastTask) { + PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take(); + if (startTask == null) { + startTask = currentTask; taskMessage = new RcsTaskMessage(this.logisticsRuntime); - startTask = nextTask; - currentTask = nextTask; - startTask.seqNo = taskMessage.SeqNo; taskMessage.OperationType = startTask.operationType; taskMessage.PickMode = startTask.pickMode; taskMessage.GoNow = true; @@ -82,33 +65,28 @@ public class PtrAgvConnectorThread extends Thread { taskMessage.StartY = startTask.startPoint.logicY; taskMessage.Link = new ArrayList<>(); } - - if (currentTask == nextTask && taskMessage != null) { - currentTask.seqNo = taskMessage.SeqNo; - RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed); - taskMessage.Link.add(link); - taskCount++; - distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]); - computingTaskList.add(currentTask); - } - - // 组织设备任务并发送 - else if (taskMessage != null && - (currentTask.operationType > 0 // 当前任务不是移动任务 - || currentTask.pickMode > 0 // 当前作业不是默认作业（无） - || nextTask.isLastTask // 下一个任务是结束任务 - || (((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction) && taskMessage.Link.size() > 0) // 下一个任务和开始任务方向不一致 - // 单向移动距离大于2m时并且点位数量大于1 - || (distance > 2 && taskCount > 1))) { - + currentTask.seqNo = taskMessage.SeqNo; + RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed); + taskMessage.Link.add(link); + taskCount++; + distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]); + computingTaskList.add(currentTask); + + + PtrAgvDeviceTask 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 + || (distance > 2 && taskCount > 1)) { taskMessage.OperationType = currentTask.operationType; taskMessage.PickMode = currentTask.pickMode; - taskMessage.EndX = currentTask.endPoint.logicX; - taskMessage.EndY = currentTask.endPoint.logicY; + taskMessage.EndX = currentTask.groupEndPoint.logicX; + taskMessage.EndY = currentTask.groupEndPoint.logicY; taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection; taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight; taskMessage.LinkCounts = (short) taskMessage.Link.size(); - + taskMessage.ChargeDirection = currentTask.chargeDirection; + taskMessage.ChargeLocation = currentTask.chargeLocation; try { // 发送任务 this.__currentTaskSeqNo = taskMessage.SeqNo; @@ -121,17 +99,26 @@ public class PtrAgvConnectorThread extends Thread { } computingTaskList.clear(); } catch (MqttException | JsonProcessingException e) { - log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.planTaskId, e); + log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.movePlanTaskId, e); } distance = 0; taskCount = 0; taskMessage = null; startTask = null; - } - if (nextTask == currentTask || nextTask.isLastTask) { - nextTask = null; + if (currentTask.isGroupEnd) { + // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒 + LockSupport.park(); // 阻塞当前线程 + } else { + for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) { + if (task.taskGroupStatus < 4) { + LockSupport.park(); // 阻塞当前线程 + break; + } + } + } } + } } catch (InterruptedException e) { System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId()); diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java index 79a93ac..0b6b70f 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java @@ -1,5 +1,8 @@ package com.yvan.logisticsModel; +import java.util.HashSet; +import java.util.Set; + public class PtrAgvDeviceTask { //该段目标点X坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 public int x; @@ -13,6 +16,10 @@ public class PtrAgvDeviceTask { public StaticItem startPoint; // 该段目标点 public StaticItem endPoint; + // 分组起始点 + public StaticItem groupStartPoint; + // 分组目标点 + public StaticItem groupEndPoint; // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业） public short operationType; // 提升移栽货物拣货模式 UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作 @@ -24,20 +31,30 @@ public class PtrAgvDeviceTask { // 目标货位相对于地面的绝对高度 public int goodsSlotHeight; + //充电桩朝向UseBriefLocation UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 + public Short chargeDirection; + // 充电工位坐标和充电桩之间距离 UInt16 单位：mm + public Integer chargeLocation; + // 目标货架编号 String 仅做校验使用（仅接货用） + public String storageRacksNo; + // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 + public Short endDirection; - /** 规划ID */ - public Long planTaskId; + /** 移动规划ID */ + public Long movePlanTaskId; + /** 转动取货放货充电等规划ID */ + public Set planTaskIdSet = new HashSet<>(); /** 业务任务ID */ public Long bizTaskId; // 作业序号 发送给小车的作业序号 public int seqNo; - // 任务状态 0创建 1发送成功 2接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更 + // 任务状态 0创建 1任务模式改变 2任务接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更 public int taskStatus = 0; public int taskGroupStatus = 0; - // 是否最后任务 - public boolean isLastTask = false; + // 任务分组结束标记 生成报文时作为判断依据 + public boolean isGroupEnd = false; public int checkLogicX; public int checkLogicY; } diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java index 98354aa..1d53083 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java @@ -16,6 +16,7 @@ import com.galaxis.rcs.ptr.sendEntity.RcsSRMessage; import com.galaxis.rcs.ptr.sendEntity.RcsSetLocationMessage; import com.google.common.base.Joiner; import com.google.common.collect.Queues; +import com.yvan.logisticsMonitor.task.PlanTask; import lombok.SneakyThrows; import lombok.extern.slf4j.Slf4j; import org.clever.core.BannerUtils; @@ -27,6 +28,7 @@ import java.util.*; import java.util.concurrent.BlockingQueue; import java.util.concurrent.CopyOnWriteArraySet; import java.util.concurrent.locks.LockSupport; +import java.util.stream.Collectors; //0.4m/ss // a max 1.2m/s //90 = 3.5s cl2 @@ -239,6 +241,29 @@ public abstract class PtrAgvItem extends ExecutorItem { return true; } + public void updateTask(int logicX, int logicY, short direction, int taskStatus) { + + updatePosition(logicX, logicY, direction); + // 查找当前分组任务 + for (PtrAgvDeviceTask task : runningDeviceTaskList) { + task.taskGroupStatus = taskStatus; + if (taskStatus == 4) { + // 更新计划任务 + List planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList(); + for (RcsTaskPlan planTask : planTaskList) { + planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); + planTaskSequence.savePlanTask(planTask); + } + } + } + + if (planTaskSequence.isAllCompleted()) { + fireEvent(AgvEventType.PLAN_COMPLETE, this); + this.runningDeviceTaskList.clear(); + planTaskSequence = null; + } + } + public void updatePosition(int logicX, int logicY, short direction) { int oldX = this.logicX; int oldY = this.logicY; @@ -273,7 +298,7 @@ public abstract class PtrAgvItem extends ExecutorItem { for (int i = 0; i < runningDeviceTaskList.size(); i++) { PtrAgvDeviceTask task = runningDeviceTaskList.get(i); - if (task.checkLogicX == logicX && task.checkLogicY == logicY) { + if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.taskStatus < 4) { finishTargetIndex = i; break; } @@ -284,13 +309,11 @@ public abstract class PtrAgvItem extends ExecutorItem { // 标记前面的任务都完成了 for (int i = 0; i <= finishTargetIndex; i++) { PtrAgvDeviceTask task = runningDeviceTaskList.get(i); - task.taskStatus = 4; // 标记为完成 - task.taskGroupStatus = 4; // 标记为任务组完成 fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task); // 更新计划任务 - RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.planTaskId); + RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId); if (planTask != null) { planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); planTaskSequence.savePlanTask(planTask); @@ -299,6 +322,7 @@ public abstract class PtrAgvItem extends ExecutorItem { if (planTaskSequence.isAllCompleted()) { fireEvent(AgvEventType.PLAN_COMPLETE, this); + this.runningDeviceTaskList.clear(); planTaskSequence = null; } } @@ -447,13 +471,16 @@ public abstract class PtrAgvItem extends ExecutorItem { 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 (RcsTaskPlan plan : sequence.taskList) { + 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())) { @@ -486,7 +513,9 @@ public abstract class PtrAgvItem extends ExecutorItem { deviceTask.startPoint = startPoint; deviceTask.endPoint = pointItem; deviceTask.bizTaskId = plan.getBizTaskId(); - deviceTask.planTaskId = plan.getPlanTaskId(); + deviceTask.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); // 行走任务完成后，检查用的字段 deviceTask.checkLogicX = pointItem.logicX; deviceTask.checkLogicY = pointItem.logicY; @@ -516,12 +545,14 @@ public abstract class PtrAgvItem extends ExecutorItem { } else if (r >= 225 && r < 315) { direction = CDirection.db; } + rotationPlanTaskIdSet.add(plan.getPlanTaskId()); } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) { 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; @@ -556,17 +587,24 @@ public abstract class PtrAgvItem extends ExecutorItem { } } + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; + } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) { 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") && storeItemRaw.containsKey("level")) { + if (storeItemRaw.containsKey("bays")) { List> bays = (List>) storeItemRaw.get("bays"); Map bay = bays.get(plan.getTargetBay()); - List levelHeight = (List) bay.get("levels"); + List levelHeight = (List) bay.get("levelHeight"); deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); } else { deviceTask.goodsSlotHeight = 1; @@ -593,11 +631,24 @@ public abstract class PtrAgvItem extends ExecutorItem { } } } + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) { PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); deviceTask.operationType = COperationType.charge; + deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm) + deviceTask.chargeDirection = 0; + deviceTask.chargeLocation = 200; + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; } if (!plan.getExecutorId().equals(this.getId())) { @@ -605,10 +656,39 @@ public abstract class PtrAgvItem extends ExecutorItem { } } - // 添加结束任务 - PtrAgvDeviceTask deviceTaskEnd = new PtrAgvDeviceTask(); - deviceTaskEnd.isLastTask = true; - deviceTaskList.add(deviceTaskEnd); + + + + // 标记任务分组结束 + PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + deviceTask.isGroupEnd = true; + // 最后一个规划任务为旋转时需要添加一个endDirection + if (rotationPlanTaskIdSet.size() > 0) { + deviceTask.endDirection = direction; + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + 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; + } + } + // planQueue.addAll(sequence.taskList); deviceTaskQueue.addAll(deviceTaskList);