cl2 基于设备状态和任务状态分步生成设备任务消息

servo/src/main/java/com/galaxis/rcs/RCS.java

@@ -188,7 +188,7 @@ public class RCS {
         planSequence.addMoveTo("22");
         planSequence.addMoveTo("21");
         planSequence.addMoveTo("20");
-        planSequence.addUnload("gst_01", 0, 0, 0);
+        planSequence.addUnload("54", 0, 0, 0);
         planSequence.addFinish();
 
         planner.appendSequence(planSequence);

servo/src/main/java/com/galaxis/rcs/communication/JacksonUtils.java

@@ -1,5 +1,6 @@
 package com.galaxis.rcs.communication;
 
+import com.fasterxml.jackson.annotation.JsonInclude;
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.core.type.TypeReference;
 import com.fasterxml.jackson.databind.JavaType;
@@ -12,7 +13,8 @@ import java.util.concurrent.ConcurrentHashMap;
 
 @Slf4j
 public class JacksonUtils {
-    private static final ObjectMapper mapper = new ObjectMapper();
+    private static final ObjectMapper mapper = new ObjectMapper().setSerializationInclusion(JsonInclude.Include.NON_NULL);
+
     private static final Map<String, JavaType> typeCache = new ConcurrentHashMap<>();
 
     // 解析泛型对象（TypeReference方式）

servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java

@@ -53,7 +53,7 @@ public class AmrMessageHandler {
 
     private static final Redis redis = AppContextHolder.getBean("defaultRedis", Redis.class, true);
 
-    public static void handleAgvRobotStatusMessage(MqttMessage message) {
+    public static void handleAgvRobotStatusMessage(MqttMessage message) throws MqttException, JsonProcessingException {
         byte[] messageData = message.getPayload();
         String json = new String(messageData, StandardCharsets.UTF_8);
         JsonWrapper jw = new JsonWrapper(json);
@@ -161,11 +161,12 @@ public class AmrMessageHandler {
                 break;
             case 20100:
                 AmrMessage<AmrHeartbeatMessage> heartbeatMessage = JacksonUtils.parse(json, typeRef20100Message);
+                sendxt(heartbeatMessage.content.VehicleId + "", heartbeatMessage.content.SeqNo + "");
 
 //                if (amrMessage.content instanceof AmrHeartbeatMessage) {
 //                    AmrHeartbeatMessage msg = (AmrHeartbeatMessage) amrMessage.content;
 //                    if (msg.VehicleId == 102) {
-//                        log.info("Received message: " + json);
+                        log.info("Received message: " + json);
 //                    }
 //                }
 
@@ -200,7 +201,7 @@ public class AmrMessageHandler {
 
     public static PtrAgvItem getPtrAgvItem(String vehicleId) {
         var runtime = LogisticsRuntimeService.INSTANCE.findByEnvCode(1L);
-        var executorItem = runtime.executorItemMap.get(vehicleId);
+        var executorItem = runtime.executorItemMap.get("10");
         return (PtrAgvItem) executorItem;
     }
     public static void sendCmd10010(String vehicleId, AmrTaskMessage amrTaskMessage) throws JsonProcessingException, MqttException {
@@ -213,4 +214,9 @@ public class AmrMessageHandler {
         mqttClient.publish("/wcs_server/" + vehicleId, json.getBytes(StandardCharsets.UTF_8), 0, false);
     }
 
+    public static void sendxt(String vehicleId, String seqNo)throws JsonProcessingException, MqttException {
+        MqttClient mqttClient = MqttConfig.mqttClient;
+        mqttClient.publish("/wcs_server/" + vehicleId, ("{\"id\": 10110, \"content\": {\"SeqNo\": " + seqNo + "}}").getBytes(), 0, false);
+    }
+
 }

servo/src/main/java/com/galaxis/rcs/connector/cl2/sendEntity/AmrTaskMessage.java

@@ -13,9 +13,9 @@ public class AmrTaskMessage {
     // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业）
     public short OperationType;
     //充电桩朝向UseBriefLocation	UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-    public short ChargeDirection;
+    public Short ChargeDirection;
     // 充电工位坐标和充电桩之间距离	UInt16 单位：mm
-    public int ChargeLocation;
+    public Integer ChargeLocation;
     // 目标货架编号 String 仅做校验使用（仅接货用）
     public String StorageRacksNo;
     // 小车起始X坐标 UInt16
@@ -27,19 +27,19 @@ public class AmrTaskMessage {
     // 小车目标Y坐标 UInt16
     public int EndY;
     // 任务是否立即执行 bool 默认为true
-    public boolean GoNow;
+    public Boolean GoNow;
     // 路径分段数	UInt8
-    public short LinkCounts;
+    public Short LinkCounts;
     // 路径分段信息
     public List<LinkData> Link;
     // AMR内置货位ID（仅对多层移栽有意义）UInt8 1～N
-    public short BuiltinSlotNo;
+    public Short BuiltinSlotNo;
     // 提升移栽货物拣货模式	UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
-    public short PickMode;
+    public Short PickMode;
     // 目标货位相对于地面的绝对高度 UInt16 单位：mm
-    public int GoodsSlotHeight;
+    public Integer GoodsSlotHeight;
     // 目标货位朝向 UInt8	朝向定义与充电桩朝向相同。 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-    public short GoodsSlotDirection;
+    public Short GoodsSlotDirection;
     // 多机构^[1]^的拣货模式 UInt8[3] 数组形式，意义同"PickMode"
     public List<Short> MPickMode;
     // 多机构^[1]^的目标货位高度 UInt16[3] 单位：mm
@@ -49,21 +49,21 @@ public class AmrTaskMessage {
     // 多机构的目标货箱ID	String[3] 仅做校验使用（仅接货用）
     public List<String> MStorageRacksNo;
     // 滚筒1 运动操作 对于左右滚动的双滚筒机型，此滚筒为靠近车头的滚筒。 对于前后滚动的双滚筒机型，此滚筒为车身左侧的滚筒。 UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
-    public short Roll1Motion;
+    public Short Roll1Motion;
     // 滚筒2 运动操作 对于左右滚动的双滚筒机型，此滚筒为靠近车尾的滚筒。 对于前后滚动的双滚筒机型，此滚筒为车身右侧的滚筒。 对于单滚筒机型，此参数无意义。  UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
-    public short Roll2Motion;
+    public Short Roll2Motion;
     // 与滚筒1对接的站台朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-    public short Roll1StationDirection;
+    public Short Roll1StationDirection;
     // 与滚筒2对接的站台朝向 对于单滚筒机型，此参数无意义。 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-    public short Roll2StationDirection;
+    public Short Roll2StationDirection;
     // 滚筒1目标货物长度 UInt16 单位：mm
-    public int Roll1GoodsLength;
+    public Integer Roll1GoodsLength;
     // 滚筒2目标货物长度 对于单滚筒机型，此参数无意义。UInt16 单位：mm
-    public int Roll2GoodsLength;
+    public Integer Roll2GoodsLength;
     // 滚筒1目标货物数量 UInt16 1～N
-    public int Roll1GoodsQuantity;
+    public Integer Roll1GoodsQuantity;
     // 滚筒2目标货物数量 对于单滚筒机型，此参数无意义。 UInt16 1～N
-    public int Roll2GoodsQuantity;
+    public Integer Roll2GoodsQuantity;
     // 多滚筒运动模式 UInt8[6]	数组形式，0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
     public List<Short> MRollMotion;
     // 多滚筒对接的站台朝向	UInt8[6] 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
@@ -71,11 +71,11 @@ public class AmrTaskMessage {
     // 多滚筒目标货物数量	UInt16[6] 数组形式，意义同"RollGoodsQuantity"
     public List<Integer> MRollGoodsQuantity;
     // 是否仅执行任务的准备部分	bool 默认为false，仅执行该动作的准备部分，如仅进行导航，调整托盘高度等，但不进行取放货操作
-    public boolean Preparing;
+    public Boolean Preparing;
     // 货架标识 uint32 车根据货架类型查询尺寸进行避障
-    public long RackTypeId;
+    public Long RackTypeId;
     // 终点自适应	 bool 默认为false，为true时，会根据任务和对应器件的位置，自动调整停止点
-    public boolean EndSelfAdaption;
+    public Boolean EndSelfAdaption;
 
 
     private static final Redis redis = AppContextHolder.getBean("defaultRedis", Redis.class, true);

servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java

@@ -49,12 +49,12 @@ public class PtrAgvConnectorThread extends Thread {
             List<PtrAgvDeviceTask> computingTaskList = new ArrayList<>();
             while (running.get()) {
 
-                for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
-                    if (task.taskGroupStatus < 3) {
-                        LockSupport.park(); // 阻塞当前线程
-                        break;
-                    }
-                }
+//                for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
+//                    if (task.taskGroupStatus < 3) {
+//                        LockSupport.park(); // 阻塞当前线程
+//                        break;
+//                    }
+//                }
 
                 // 从队列中获取任务，如果队列为空则阻塞等待
                 if (nextTask == null) {
@@ -99,6 +99,9 @@ public class PtrAgvConnectorThread extends Thread {
                     taskMessage.PickMode = currentTask.pickMode;
                     taskMessage.EndX = currentTask.endPoint.logicX;
                     taskMessage.EndY = currentTask.endPoint.logicY;
+                    taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection;
+                    taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight;
+                    taskMessage.LinkCounts = (short)taskMessage.Link.size();
 
                     try {
                         // 发送任务

servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java

@@ -18,6 +18,13 @@ public class PtrAgvDeviceTask {
     // 提升移栽货物拣货模式	UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
     public short pickMode;
 
+    // 目标货位朝向
+    public short goodsSlotDirection;
+
+    // 目标货位相对于地面的绝对高度
+    public int goodsSlotHeight;
+
+
     /** 规划ID */
     public Long planTaskId;
     /** 业务任务ID */

servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java

@@ -6,6 +6,7 @@ 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 org.clever.core.Conv;
 import org.clever.core.json.JsonWrapper;
 
 import java.util.ArrayList;
@@ -155,13 +156,15 @@ public class PtrAgvItem extends ExecutorItem {
 
         // 生成移动报文
         List<PtrAgvDeviceTask> deviceTaskList = new ArrayList<>();
-
+        List<Map<String, Object>> linkStore = null;
         // 检查 planList 是不是全都是我的任务
         for (RcsTaskPlan plan : sequence.taskList) {
             String endPointId = plan.getTargetId();
+
             if (plan.getPlanType().equals(PlanTaskType.MOVE.toString())) {
                 // 获取目标点信息
                 StaticItem pointItem = runtime.getStaticItemById(endPointId);
+                linkStore = (List<Map<String, Object>>) 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;
@@ -221,15 +224,82 @@ public class PtrAgvItem extends ExecutorItem {
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.transplantLoadAndUnload;
                 deviceTask.pickMode = CPickMode.load;
+                //处理取货高度
+                StaticItem storeItem = runtime.getStaticItemById(endPointId);
+                Map<String, Object> storeItemRaw = storeItem.dt;
+                if (storeItemRaw.containsKey("bays")) {
+                    List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
+                    Map<String, Object> bay = bays.get(plan.getTargetBay());
+                    List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
+                    deviceTask.goodsSlotHeight = (int)Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
+                } else {
+                    deviceTask.goodsSlotHeight = 1;
+                }
+                if (linkStore != null) {
+                    for (Map<String, Object> 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;
+                        }
+                    }
+                }
 
             } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.transplantLoadAndUnload;
                 deviceTask.pickMode = CPickMode.unload;
+                // 处理卸货高度
+                StaticItem storeItem = runtime.getStaticItemById(endPointId);
+                Map<String, Object> storeItemRaw = storeItem.dt;
+                if (storeItemRaw.containsKey("bays") && storeItemRaw.containsKey("level")) {
+                    List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
+                    Map<String, Object> bay = bays.get(plan.getTargetBay());
+                    List<Double> levelHeight = (List<Double>) bay.get("levels");
+                    deviceTask.goodsSlotHeight = (int)Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
+                } else {
+                    deviceTask.goodsSlotHeight = 1;
+                }
+                if (linkStore != null) {
+                    for (Map<String, Object> 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;
+                        }
+                    }
+                }
 
             } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.charge;
+                // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
             }
 
             if (!plan.getExecutorId().equals(this.getId())) {