diff --git a/servo/src/main/java/com/galaxis/rcs/RCS.java b/servo/src/main/java/com/galaxis/rcs/RCS.java index 02df368..0ecbc50 100644 --- a/servo/src/main/java/com/galaxis/rcs/RCS.java +++ b/servo/src/main/java/com/galaxis/rcs/RCS.java @@ -30,15 +30,21 @@ public class RCS { } @SneakyThrows - static void init() { + public static void init() { String fs = Joiner.on("\n").join(FileUtils.readLines(new File("./yvan-rcs-web/src/example/example1.json"), StandardCharsets.UTF_8)); JsonWrapper jw = new JsonWrapper(fs); + if (LogisticsRuntimeService.findByEnvCode(1L) == null) { + LogisticsRuntimeService.createEnv(1); + LogisticsRuntime runtime = LogisticsRuntimeService.findByEnvCode(1); + runtime.loadMap(jw); - LogisticsRuntimeService.createEnv(1); - LogisticsRuntime runtime = LogisticsRuntimeService.findByEnvCode(1); - runtime.loadMap(jw); + runtime.start(); + } - runtime.start(); + } + + public static void ok() + { } /** diff --git a/servo/src/main/java/com/galaxis/rcs/common/entity/RcsTaskPlan.java b/servo/src/main/java/com/galaxis/rcs/common/entity/RcsTaskPlan.java index 2b8dce2..6b9189c 100644 --- a/servo/src/main/java/com/galaxis/rcs/common/entity/RcsTaskPlan.java +++ b/servo/src/main/java/com/galaxis/rcs/common/entity/RcsTaskPlan.java @@ -30,7 +30,7 @@ public class RcsTaskPlan implements Serializable { private Integer targetLevel; /** 目标点货架格 */ private Integer targetCell; - /** 目标点货架旋转角度 */ + /** 目标点旋转角度 */ private BigDecimal targetRotation; /** 背负托盘ID */ private String loadLpn; diff --git a/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java index a38bcc0..e1b711d 100644 --- a/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java +++ b/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java @@ -3,6 +3,7 @@ package com.galaxis.rcs.communication.amrCommunication; import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.core.type.TypeReference; +import com.galaxis.rcs.RCS; import com.galaxis.rcs.communication.JacksonUtils; import com.galaxis.rcs.connector.cl2.receiveEntity.*; import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskCompletedData; @@ -185,12 +186,16 @@ public class AmrMessageHandler { } } + static { + RCS.ok(); + } + public static PtrAgvItem getPtrAgvItem(String vehicleId) { var runtime = LogisticsRuntimeService.findByEnvCode(1L); var executorItem = runtime.executorItemMap.get(vehicleId); return (PtrAgvItem) executorItem; } - public static void sendCmd10010(int vehicleId, AmrTaskMessage amrTaskMessage) throws JsonProcessingException, MqttException { + public static void sendCmd10010(String vehicleId, AmrTaskMessage amrTaskMessage) throws JsonProcessingException, MqttException { MqttClient mqttClient = AppContextHolder.getBean(MqttClient.class, true); String json = JacksonUtils.toJson(amrTaskMessage); mqttClient.publish("/wcs_server/" + vehicleId, json.getBytes(StandardCharsets.UTF_8), 0, false); diff --git a/servo/src/main/java/com/galaxis/rcs/connector/ConnectorConsumer.java b/servo/src/main/java/com/galaxis/rcs/connector/ConnectorConsumer.java deleted file mode 100644 index fdae98d..0000000 --- a/servo/src/main/java/com/galaxis/rcs/connector/ConnectorConsumer.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.galaxis.rcs.connector; - -import com.galaxis.rcs.common.entity.RcsTaskPlan; -import com.yvan.logisticsModel.ExecutorItem; -import com.yvan.logisticsModel.LogisticsRuntime; - -public interface ConnectorConsumer { - void consume(ExecutorItem executorItem, LogisticsRuntime logisticsRuntime, RcsTaskPlan task); -} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/ConnectorFactory.java b/servo/src/main/java/com/galaxis/rcs/connector/ConnectorFactory.java deleted file mode 100644 index 9d09cc4..0000000 --- a/servo/src/main/java/com/galaxis/rcs/connector/ConnectorFactory.java +++ /dev/null @@ -1,40 +0,0 @@ -package com.galaxis.rcs.connector; - -import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; -import com.galaxis.rcs.connector.cl2.VirtualCl2Connector; -import com.google.common.collect.Maps; - -import java.util.Map; - -public class ConnectorFactory { - /** - * 设备连接器映射 - */ - private static final Map connectorMap = Maps.newHashMap(); - - /** - * 虚拟连接器映射 - */ - private static final Map virtualConnectorMap = Maps.newHashMap(); - - static { - connectorMap.put("cl2", new Cl2DeviceConnector()); - virtualConnectorMap.put("cl2", new VirtualCl2Connector()); - } - - public static ConnectorConsumer getConnector(String type) { - ConnectorConsumer connector = connectorMap.get(type); - if (connector == null) { - throw new IllegalArgumentException("No connector found for type: " + type); - } - return connector; - } - - public static ConnectorConsumer getVirtualConnector(String type) { - ConnectorConsumer connector = connectorMap.get(type); - if (connector == null) { - throw new IllegalArgumentException("No connector found for type: " + type); - } - return connector; - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Connector.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Connector.java deleted file mode 100644 index 4e9c9b9..0000000 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Connector.java +++ /dev/null @@ -1,103 +0,0 @@ -package com.galaxis.rcs.connector.cl2; - -import com.galaxis.rcs.common.entity.RcsTaskPlan; -import com.galaxis.rcs.common.enums.PlanTaskType; -import com.galaxis.rcs.connector.ConnectorConsumer; -import com.yvan.logisticsModel.ExecutorItem; -import com.yvan.logisticsModel.LogisticsRuntime; -import com.yvan.logisticsModel.PtrAgvItem; - -public abstract class Cl2Connector implements ConnectorConsumer { - // RCS发往CL2的指令 - - - /** - * 消费任务计划 - * - * @param executorItem 机器人执行器 - * @param runtime 物流运行时环境 - * @param task 任务计划 - */ - @Override - public void consume(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task) { - - PtrAgvItem ptrAgvItem = (PtrAgvItem) executorItem; - - switch (PlanTaskType.valueOf(task.getPlanType())) { - case MOVE: - robotMove(ptrAgvItem, runtime, task, task.getTargetId()); - break; - case ROTATION: - robotRotation(ptrAgvItem, runtime, task, task.getTargetRotation().floatValue()); - break; - case LOAD: - robotLoad(ptrAgvItem, runtime, task, task.getTargetId(), task.getTargetBay(), task.getTargetLevel(), task.getTargetCell()); - break; - case UNLOAD: - robotUnload(ptrAgvItem, runtime, task, task.getTargetId(), task.getTargetBay(), task.getTargetLevel(), task.getTargetCell()); - break; - case CHARGE: - robotCharger(ptrAgvItem, runtime, task, task.getTargetId()); - break; - default: - throw new IllegalArgumentException("Unknown plan type: " + task.getPlanType()); - } - } - - /** - * 移动 robotMove - * - * @param ptrAgvItem 机器人执行器 - * @param runtime 物流运行时环境 - * @param task 任务计划 - * @param endWayPoint 终点ID, WayPointId - */ - public abstract void robotMove(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String endWayPoint); - - /** - * 旋转 robotRotate - * - * @param ptrAgvItem 机器人执行器 - * @param runtime 物流运行时环境 - * @param task 任务计划 - * @param worldRotation 转动身体到世界角度 90 度 - */ - public abstract void robotRotation(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation); - - /** - * 取货 robotLoad - * - * @param ptrAgvItem 机器人执行器 - * @param runtime 物流运行时环境 - * @param task 任务计划 - * @param rackItem 货架ID - * @param bay 列 - * @param level 层 - * @param cell 格 - */ - public abstract void robotLoad(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell); - - /** - * 放货 robotUnload - * - * @param ptrAgvItem 机器人执行器 - * @param runtime 物流运行时环境 - * @param task 任务计划 - * @param rackItem 货架ID - * @param bay 列 - * @param level 层 - * @param cell 格 - */ - public abstract void robotUnload(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell); - - /** - * 充电 robotCharge - * - * @param ptrAgvItem 机器人执行器 - * @param runtime 物流运行时环境 - * @param task 任务计划 - * @param chargerItem 充电桩ID - */ - public abstract void robotCharger(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem); - -} diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java index c12f35a..1ca00e8 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java @@ -1,11 +1,13 @@ package com.galaxis.rcs.connector.cl2; +import com.fasterxml.jackson.core.JsonProcessingException; import com.galaxis.rcs.common.entity.RcsTaskPlan; import com.galaxis.rcs.communication.amrCommunication.AmrMessageHandler; import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage; import com.yvan.logisticsModel.*; import lombok.extern.slf4j.Slf4j; import org.clever.core.Conv; +import org.eclipse.paho.mqttv5.common.MqttException; import java.math.BigDecimal; @@ -13,82 +15,10 @@ import java.math.BigDecimal; * CL2 车型报文推送 */ @Slf4j -public class Cl2DeviceConnector extends Cl2Connector { +public class Cl2DeviceConnector { - @Override - public void robotMove(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String endPointId) { - - double speed = 1000; - - // 获取目标点信息 - StaticItem pointItem = runtime.getStaticItemById(endPointId); - - - // 生成移动报文 - AmrTaskMessage message = new AmrTaskMessage(); - message.SeqNo = 1; - message.OperationType = 0; - message.GoNow = true; - message.StartX = ptrAgvItem.logicX; - message.StartY = ptrAgvItem.logicY; - message.EndX = pointItem.logicX; - message.EndY = pointItem.logicY; - double targetRotation = task.getTargetRotation().doubleValue(); - while (targetRotation > 360) { - targetRotation -= 360; - } - while (targetRotation < 0) { - targetRotation += 360; - } - - short targetDirection = (short) (Math.round(targetRotation/90) % 4); - - // 判断车的 速度方向 - if (ptrAgvItem.direction == targetDirection) { - - } - - - - log.info("Cl2DeviceConnector robotMove: executorItem={}, task={}, endPointId={}", - ptrAgvItem.getId(), task.getPlanTaskId(), endPointId); - - // 获取静态数据（货架、地标等） - -// log.info("{}", pointItem); - - // 获取车的数据 -// ExecutorItem agv = runtime.executorItemMap.get(executorItem.getId()); -// log.info("{}", agv); - - // 获取箱子的数据 - FlowItem box = runtime.flowItemMap.get("pallet1122"); - log.info("{}", box); - - log.info("车的当前位置是不可知的，要从实际情况中获取, 移动的路径一定是可以到达的，不用考虑转向问题。但 Speed 是正还是负，需要从当前车的角度情况下决定。"); - } - - @Override - public void robotRotation(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation) { - log.info("Cl2DeviceConnector robotRotation: executorItem={}, task={}, worldRotation={}", - ptrAgvItem.getId(), task.getPlanTaskId(), worldRotation); + public void sendTask(String vehicleId, AmrTaskMessage amrTaskMessage) throws MqttException, JsonProcessingException { + AmrMessageHandler.sendCmd10010(vehicleId, amrTaskMessage); } - @Override - public void robotLoad(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) { - log.info("Cl2DeviceConnector robotLoad: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}", - ptrAgvItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell); - } - - @Override - public void robotUnload(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) { - log.info("Cl2DeviceConnector robotUnload: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}", - ptrAgvItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell); - } - - @Override - public void robotCharger(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem) { - log.info("Cl2DeviceConnector robotCharger: executorItem={}, task={}, chargerItem={}", - ptrAgvItem.getId(), task.getPlanTaskId(), chargerItem); - } } diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java index 6f49601..28ea82c 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java @@ -6,39 +6,7 @@ import com.yvan.logisticsModel.LogisticsRuntime; import com.yvan.logisticsModel.PtrAgvItem; import lombok.extern.slf4j.Slf4j; -/** - * @author cwj - * @since 2023/7/30 - */ @Slf4j -public class VirtualCl2Connector extends Cl2Connector { - @Override - public void robotMove(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String endWayPoint) { - log.info("VirtualCl2Connector robotMove: executorItem={}, task={}, endWayPoint={}", - ptrAgvItem.getId(), task.getPlanTaskId(), endWayPoint); - } +public class VirtualCl2Connector { - @Override - public void robotRotation(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation) { - log.info("VirtualCl2Connector robotRotation: executorItem={}, task={}, worldRotation={}", - ptrAgvItem.getId(), task.getPlanTaskId(), worldRotation); - } - - @Override - public void robotLoad(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) { - log.info("VirtualCl2Connector robotLoad: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}", - ptrAgvItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell); - } - - @Override - public void robotUnload(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) { - log.info("VirtualCl2Connector robotUnload: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}", - ptrAgvItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell); - } - - @Override - public void robotCharger(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem) { - log.info("VirtualCl2Connector robotCharger: executorItem={}, task={}, chargerItem={}", - ptrAgvItem.getId(), task.getPlanTaskId(), chargerItem); - } } diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/sendEntity/AmrTaskMessage.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/sendEntity/AmrTaskMessage.java index fddedb8..5ad9620 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/sendEntity/AmrTaskMessage.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/sendEntity/AmrTaskMessage.java @@ -1,10 +1,15 @@ package com.galaxis.rcs.connector.cl2.sendEntity; +import lombok.AllArgsConstructor; +import org.clever.core.AppContextHolder; +import org.clever.core.Conv; +import org.clever.data.redis.Redis; + import java.util.List; public class AmrTaskMessage { // 作业序号 UInt32 - public long SeqNo; + public int SeqNo; // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业） public short OperationType; //充电桩朝向UseBriefLocation UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 @@ -72,7 +77,22 @@ public class AmrTaskMessage { // 终点自适应 bool 默认为false，为true时，会根据任务和对应器件的位置，自动调整停止点 public boolean EndSelfAdaption; - static class Link { + + private static final Redis redis = AppContextHolder.getBean("defaultRedis", Redis.class, true); + + private static final String REDIS_KEY = "amr:task:message:seqNo"; + + public AmrTaskMessage() { + long seqNo = redis.vIncrement(REDIS_KEY); + if (seqNo > Integer.MAX_VALUE) { + redis.kDelete(REDIS_KEY); + seqNo = redis.vIncrement(REDIS_KEY).intValue(); + } + this.SeqNo = (int)seqNo; + } + + @AllArgsConstructor + public static class Link { //该段目标点X坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 public int X; //该段目标点Y坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 diff --git a/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java b/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java index cb719bb..00186b8 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java +++ b/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java @@ -50,4 +50,14 @@ public abstract class BaseItem { this.dt = (Map) map.get("dt"); } + + public float getTransformationX() { + return tf[0][0]; + } + public float getTransformationY() { + return tf[0][2]; + } + public float getTransformationZ() { + return tf[0][1]; + } } diff --git a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntimeService.java b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntimeService.java index 6dca652..648aaf9 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntimeService.java +++ b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntimeService.java @@ -21,7 +21,8 @@ public class LogisticsRuntimeService { if (runtime != null) { return runtime; } - throw new RuntimeException("LogisticsRuntime not found for envCode: " + envId); + return null; +// throw new RuntimeException("LogisticsRuntime not found for envCode: " + envId); } } diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java new file mode 100644 index 0000000..de74dbb --- /dev/null +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java @@ -0,0 +1,7 @@ +package com.yvan.logisticsModel; + +import com.galaxis.rcs.common.entity.RcsTaskPlan; + +public abstract class PtrAgvConnector { + +} diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java index 279a0c4..c37758b 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java @@ -1,10 +1,12 @@ package com.yvan.logisticsModel; -import com.galaxis.rcs.common.entity.RcsTaskPlan; -import com.galaxis.rcs.common.enums.PlanTaskStatus; -import com.galaxis.rcs.connector.ConnectorFactory; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; +import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage; import lombok.extern.slf4j.Slf4j; +import org.eclipse.paho.mqttv5.common.MqttException; +import java.util.ArrayList; import java.util.concurrent.atomic.AtomicBoolean; @Slf4j @@ -12,11 +14,17 @@ public class PtrAgvConnectorThread extends Thread { private final AtomicBoolean running = new AtomicBoolean(false); private final PtrAgvItem executorItem; + private final Cl2DeviceConnector cl2DeviceConnector; + + private final PtrAgvItem ptrAgvItem; + private final LogisticsRuntime logisticsRuntime; - public PtrAgvConnectorThread(PtrAgvItem executorItem, LogisticsRuntime logisticsRuntime) { + public PtrAgvConnectorThread(PtrAgvItem executorItem, Cl2DeviceConnector cl2DeviceConnector, PtrAgvItem ptrAgvItem, LogisticsRuntime logisticsRuntime) { super("ExecutorConnector-" + executorItem.getId()); this.executorItem = executorItem; + this.cl2DeviceConnector = cl2DeviceConnector; + this.ptrAgvItem = ptrAgvItem; this.logisticsRuntime = logisticsRuntime; } @@ -26,19 +34,57 @@ public class PtrAgvConnectorThread extends Thread { log.info("Connector thread started for executor: {}", this.executorItem.getId()); try { + + float distance = 0; + int taskCount = 0; + Boolean isPositiveSpeed = null; + short headerDirection = -1; + PtrAgvDeviceTask startTask = null; + + AmrTaskMessage taskMessage = null; + while (running.get()) { // 从队列中获取任务，如果队列为空则阻塞等待 - RcsTaskPlan task = this.executorItem.planQueue.take(); + PtrAgvDeviceTask deviceTask = this.executorItem.deviceTaskQueue.take(); + if (startTask == null) { + startTask = deviceTask; + isPositiveSpeed = deviceTask.speed > 0; + headerDirection = deviceTask.direction; + taskMessage = new AmrTaskMessage(); + taskMessage.OperationType = deviceTask.operationType; + taskMessage.PickMode = deviceTask.pickMode; + taskMessage.GoNow = true; + taskMessage.StartX = deviceTask.startPoint.logicX; + taskMessage.StartY = deviceTask.startPoint.logicY; + taskMessage.Links = new ArrayList<>(); + + } + + deviceTask.seqNo = taskMessage.SeqNo; + + AmrTaskMessage.Link link = new AmrTaskMessage.Link(deviceTask.endPoint.logicX, deviceTask.endPoint.logicY, deviceTask.speed); + taskMessage.Links.add(link); - try { - // 处理任务 - ConnectorFactory.getConnector(this.executorItem.getT()) - .consume(this.executorItem, this.logisticsRuntime, task); + if (isPositiveSpeed == (deviceTask.speed > 0) && headerDirection == deviceTask.direction) { + taskCount ++; + distance += euclideanDistance(deviceTask.startPoint.tf[0], deviceTask.endPoint.tf[0]); - } catch (Exception e) { - log.error("Error processing task: " + task, e); - task.setPlanTaskErrorInfo(e.getMessage()); - task.setPlanTaskStatus(PlanTaskStatus.ERROR.toString()); + } + if((isPositiveSpeed != (deviceTask.speed > 0) || headerDirection != deviceTask.direction) + // 单向移动距离大于2m时拆分任务指令 + || (distance > 2 && taskCount > 1)) { + taskMessage.EndX = deviceTask.endPoint.logicX; + taskMessage.EndY = deviceTask.endPoint.logicY; + distance = 0; + taskCount = 0; + startTask = null; + taskMessage = null; + try { + cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage); + } catch (MqttException | JsonProcessingException e) { + log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, error={}", + ptrAgvItem.getId(), deviceTask.planTaskId, e); + } } } } catch (InterruptedException e) { @@ -62,4 +108,12 @@ public class PtrAgvConnectorThread extends Thread { public boolean isRunning() { return running.get(); } + + public static double euclideanDistance(float[] p1, float[] p2) { + if (p1.length != 3 || p2.length != 3) throw new IllegalArgumentException(); + float dx = p1[0] - p2[0]; + float dy = p1[1] - p2[1]; + float dz = p1[2] - p2[2]; + return Math.sqrt(dx * dx + dy * dy + dz * dz); + } } diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java new file mode 100644 index 0000000..ed6bd44 --- /dev/null +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java @@ -0,0 +1,27 @@ +package com.yvan.logisticsModel; + +public class PtrAgvDeviceTask { + //该段目标点X坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 + public int x; + //该段目标点Y坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 + public int y; + // 该段行驶速度 Int16 mm/s + public int speed; + // 该段车头朝向 + public short direction; + // 该段起始点 + public StaticItem startPoint; + // 该段目标点 + public StaticItem endPoint; + // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业） + public short operationType; + // 提升移栽货物拣货模式 UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作 + public short pickMode; + + /** 规划ID */ + public Long planTaskId; + /** 业务任务ID */ + public Long bizTaskId; + // 作业序号 发送给小车的作业序号 + public int seqNo; +} diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java index c5a9561..8ec069d 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java @@ -1,15 +1,25 @@ package com.yvan.logisticsModel; import com.galaxis.rcs.common.entity.RcsTaskPlan; +import com.galaxis.rcs.common.enums.PlanTaskType; +import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; import com.galaxis.rcs.plan.PlanTaskSequence; import com.google.common.collect.Queues; +import lombok.extern.slf4j.Slf4j; +import java.util.ArrayList; +import java.util.List; import java.util.Map; import java.util.concurrent.BlockingQueue; - +//0.4m/ss // a max 1.2m/s +//90 = 3.5s cl2 +//90 = 5s // cLX +@Slf4j public class PtrAgvItem extends ExecutorItem { private final int BLOCKING_QUEUE_CAPACITY = 100; + private final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(); + // ip public String ip; // agv名称 @@ -53,6 +63,8 @@ public class PtrAgvItem extends ExecutorItem { */ final BlockingQueue planQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY); + final BlockingQueue deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY); + /** * 连接器线程 @@ -76,6 +88,8 @@ public class PtrAgvItem extends ExecutorItem { System.out.println("Connector stopped for executor: " + this.getId()); } + private static final int speed = 1000; + /** * 添加任务序列到当前执行器 */ @@ -83,28 +97,146 @@ public class PtrAgvItem extends ExecutorItem { if (sequence == null || sequence.taskList.isEmpty()) { return; } + LogisticsRuntime runtime = sequence.logisticsRuntime; + + + short direction = this.direction; + + // 获取当前设备点位(逻辑点位) + StaticItem startPoint = runtime.getStaticItemByLogicXY(this.logicX, this.logicY); + if (startPoint == null) { + log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId()); + } + + + // 生成移动报文 + List deviceTaskList = new ArrayList<>(); // 检查 planList 是不是全都是我的任务 for (RcsTaskPlan plan : sequence.taskList) { + String endPointId = plan.getTargetId(); + if (plan.getPlanType().equals(PlanTaskType.MOVE.toString())) { + // 获取目标点信息 + StaticItem pointItem = runtime.getStaticItemById(endPointId); + int d = -1; + if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) { + d = pointItem.logicY > startPoint.logicY ? CDirection.db : CDirection.dt; + if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) { + throw new RuntimeException("方向错误"); + } + + } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) { + d = pointItem.logicX > startPoint.logicX ? CDirection.dr : CDirection.dl; + if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) { + throw new RuntimeException("方向错误"); + } + // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX()); + + } else { + throw new RuntimeException("无法识别的点位关系"); + } + PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); + deviceTask.x = pointItem.logicX; + deviceTask.y = pointItem.logicY; + deviceTask.speed = d == direction ? (speed) : (-speed); + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = pointItem; + deviceTask.bizTaskId = plan.getBizTaskId(); + deviceTask.planTaskId = plan.getPlanTaskId(); + deviceTaskList.add(deviceTask); + // 设置新的起点 + startPoint = pointItem; + + } else if (plan.getPlanType().equals(PlanTaskType.ROTATION.toString())) { + + float r = plan.getTargetRotation().floatValue(); + while (r > 360) { + r -= 360; + } + while (r < 0) { + r += 360; + } + + if (r >= 315 || r < 45) { + direction = CDirection.dr; + + } else if (r >= 45 && r < 135) { + direction = CDirection.dt; + + } else if (r >= 135 && r < 225) { + direction = CDirection.dl; + + } else if (r >= 225 && r < 315) { + direction = CDirection.db; + } + + } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) { + + PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = COperationType.transplantLoadAndUnload; + deviceTask.pickMode = CPickMode.load; + + } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) { + PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = COperationType.transplantLoadAndUnload; + deviceTask.pickMode = CPickMode.unload; + + } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) { + PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = COperationType.charge; + } + if (!plan.getExecutorId().equals(this.getId())) { throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId()); } - } - LogisticsRuntime runtime = sequence.logisticsRuntime; + } planQueue.addAll(sequence.taskList); + deviceTaskQueue.addAll(deviceTaskList); // TODO: 开启轮询线程，等待下一个待执行任务 } public boolean isFree() { - return (this.planQueue.isEmpty() && this.connectorThread.isRunning()); + return (this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning()); } public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map raw) { super(logisticsRuntime, raw); - this.connectorThread = new PtrAgvConnectorThread(this, logisticsRuntime); + this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, this, logisticsRuntime); } + + private static class CDirection { + private static final short dr = 0; + private static final short db = 1; + private static final short dl = 2; + private static final short dt = 3; + } + + + + private static class COperationType { + public static final short move = 0; + public static final short load = 1; + public static final short unpick = 2; + public static final short charge = 3; + public static final short transplantLoadAndUnload = 4; + public static final short rollerLoadAndUnload = 5; + } + + private static class CPickMode { + public static final short normal = 0; + public static final short load = 1; + public static final short unload = 2; + public static final short adjustHeight = 3; + public static final short adjustHeightToLoad = 5; + public static final short adjustHeightToUnload = 6; + + } + + }