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

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

@@ -10,7 +10,8 @@ import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskCompletedData;
 import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskModeChangeData;
 import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskStatusChangeData;
 import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskTypeChangeData;
-import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage;
+import com.galaxis.rcs.connector.cl2.sendEntity.RcsAckMessage;
+import com.galaxis.rcs.connector.cl2.sendEntity.RcsTaskMessage;
 import com.galaxis.rcs.connector.cl2.sendEntity.BaseMessage;
 import com.yvan.logisticsModel.LogisticsRuntimeService;
 import com.yvan.logisticsModel.PtrAgvItem;
@@ -29,6 +30,66 @@ import java.nio.charset.StandardCharsets;
 @Service
 public class AmrMessageHandler {
 
+    // 发送消息id
+    /**
+     * AMR作业指令 10010
+     * 停止/解除 10040
+     * 电文应答 10050
+     * 配置信息 10060
+     * 旋转货架 10080
+     * 旋转车身 10081
+     * 控制卷帘门 10082
+     * 心跳 10100
+     * 状态查询 10110
+     * 取消已下发小车任务 10120
+     * 设置小车坐标 10200
+     * 等待就绪 19997
+     */
+    private static final int RCS_TASK = 10010;
+    private static final int RCS_SR = 10040;
+    private static final int RCS_ACK = 10050;
+    private static final int RCS_CONFIG = 10060;
+    private static final int RCS_ROTATE_RACK = 10080;
+    private static final int RCS_ROTATE_BODY = 10081;
+    private static final int RCS_CONTROL_DOOR = 10082;
+    private static final int RCS_HEARTBEAT = 10100;
+    private static final int RCS_QUERY_STATUS = 10110;
+    private static final int RCS_CANCEL_TASK = 10120;
+    private static final int RCS_SET_LOCATION = 10200;
+    private static final int RCS_WAIT_READY = 19997;
+
+    // 接收消息id
+    /**
+     * 小车作业完成 20010 AmrTaskCompletedMessage
+     * 任务状态上报 20011 AmrTaskStatusMessage
+     * 小车子模块任务状态 20012 AmrModuleTaskStatusMessage
+     * 地标报告 20020 AmrLandmarkMessage
+     * 消息应答 20050 AmrAckMessage
+     * 状态上报 20060 AmrStatusMessage
+     * 心跳 20100 AmrHeartbeatMessage
+     * 开机上报 20147 AmrBootMessage
+     * 关机上报 20148 AmrShutdownMessage
+     * 小车主程序启动 20149 AmrAppStartMessage
+     * 小车上线 20150 AmrOnlineMessage
+     * 小车离线 20200 AmrOfflineMessage
+     * 异常上报 20250 AmrExceptionMessage
+     */
+    private static final int AMR_TASK_COMPLETED = 20010;
+    private static final int AMR_TASK_STATUS = 20011;
+    private static final int AMR_MODULE_TASK_STATUS = 20012;
+    private static final int AMR_LANDMARK = 20020;
+    private static final int AMR_ACK = 20050;
+    private static final int AMR_STATUS = 20060;
+    private static final int AMR_HEARTBEAT = 20100;
+    private static final int AMR_BOOT = 20147;
+    private static final int AMR_SHUTDOWN = 20148;
+    private static final int AMR_APP_START = 20149;
+    private static final int AMR_ONLINE = 20150;
+    private static final int AMR_OFFLINE = 20200;
+    private static final int AMR_EXCEPTION = 20250;
+
+
+
     private static final TypeReference<AmrMessage<AmrTaskCompletedMessage>> typeRef20010Message = new TypeReference<AmrMessage<AmrTaskCompletedMessage>>() {};
     private static final TypeReference<AmrMessage<AmrTaskStatusMessage<TaskStatusChangeData>>> typeRef20011_defaultMessage = new TypeReference<AmrMessage<AmrTaskStatusMessage<TaskStatusChangeData>>>() {};
     private static final TypeReference<AmrMessage<AmrTaskStatusMessage<TaskModeChangeData>>> typeRef20011_1Message = new TypeReference<AmrMessage<AmrTaskStatusMessage<TaskModeChangeData>>>() {};
@@ -79,10 +140,10 @@ public class AmrMessageHandler {
 
         AmrMessage<?> amrMessage = null;
         switch (id) {
-            case 20010:
+            case AMR_TASK_COMPLETED:
                 AmrMessage<AmrTaskCompletedMessage> taskCompletedMessage = JacksonUtils.parse(json, typeRef20010Message);
                 break;
-            case 20011: {
+            case AMR_TASK_STATUS: {
                 int EventId = jw.asInt("content", "EventId");
                 log.info("1-Received message: " + json);
                 switch (EventId) {
@@ -119,10 +180,10 @@ public class AmrMessageHandler {
                 }
                 break;
             }
-            case 20012:
+            case AMR_MODULE_TASK_STATUS:
                 amrMessage = JacksonUtils.parse(json, typeRef20012Message);
                 break;
-            case 20020:
+            case AMR_LANDMARK:
                 AmrMessage<AmrLandmarkMessage> landmarkMessage = JacksonUtils.parse(json, typeRef20020Message);
                 agvItem.x = landmarkMessage.content.X;
                 agvItem.y = landmarkMessage.content.Y;
@@ -134,10 +195,10 @@ public class AmrMessageHandler {
 //                    log.info("2-Received message: " + json);
 //                }
                 break;
-            case 20050:
+            case AMR_ACK:
                 amrMessage = JacksonUtils.parse(json, typeRef20050Message);
                 break;
-            case 20060:
+            case AMR_STATUS:
                 AmrMessage<AmrStatusMessage> statusMessage = JacksonUtils.parse(json, typeRef20060Message);
                 agvItem.agvSOC = statusMessage.content.CurBattery.SOC;
                 agvItem.agvBatteryVoltage = statusMessage.content.CurBattery.Voltage;
@@ -154,7 +215,7 @@ public class AmrMessageHandler {
 //                        log.info("Received message: " + json);
 //                    }
                 break;
-            case 20100:
+            case AMR_HEARTBEAT:
                 AmrMessage<AmrHeartbeatMessage> heartbeatMessage = JacksonUtils.parse(json, typeRef20100Message);
                 sendxt(heartbeatMessage.content.VehicleId + "", heartbeatMessage.content.SeqNo + "");
 
@@ -166,22 +227,22 @@ public class AmrMessageHandler {
 //                }
 
                 break;
-            case 20147:
+            case AMR_BOOT:
                 amrMessage = JacksonUtils.parse(json, typeRef20147Message);
                 break;
-            case 20148:
+            case AMR_SHUTDOWN:
                 amrMessage = JacksonUtils.parse(json, typeRef20148Message);
                 break;
-            case 20149:
+            case AMR_APP_START:
                 amrMessage = JacksonUtils.parse(json, typeRef20149Message);
                 break;
-            case 20150:
+            case AMR_ONLINE:
                 amrMessage = JacksonUtils.parse(json, typeRef20150Message);
                 break;
-            case 20200:
+            case AMR_OFFLINE:
                 amrMessage = JacksonUtils.parse(json, typeRef20200Message);
                 break;
-            case 20250:
+            case AMR_EXCEPTION:
                 amrMessage = JacksonUtils.parse(json, typeRef20250Message);
                 break;
             default:
@@ -196,14 +257,14 @@ public class AmrMessageHandler {
 
     public static PtrAgvItem getPtrAgvItem(String vehicleId) {
         var runtime = LogisticsRuntimeService.INSTANCE.findByEnvCode(1L);
-        var executorItem = runtime.executorItemMap.get("10");
+        var executorItem = runtime.executorItemMap.get("3");
         return (PtrAgvItem) executorItem;
     }
-    public static void sendCmd10010(String vehicleId, AmrTaskMessage amrTaskMessage) throws JsonProcessingException, MqttException {
+    public static void sendCmd10010(String vehicleId, RcsTaskMessage rcsTaskMessage) throws JsonProcessingException, MqttException {
         MqttClient mqttClient = MqttConfig.mqttClient;
         BaseMessage baseMessage = new BaseMessage();
         baseMessage.id = 10010;
-        baseMessage.content = amrTaskMessage;
+        baseMessage.content = rcsTaskMessage;
         String json = JacksonUtils.toJson(baseMessage);
         log.info("sendCmd10010: {}", json);
         mqttClient.publish("/wcs_server/" + vehicleId, json.getBytes(StandardCharsets.UTF_8), 0, false);
@@ -214,4 +275,16 @@ public class AmrMessageHandler {
         mqttClient.publish("/wcs_server/" + vehicleId, ("{\"id\": 10110, \"content\": {\"SeqNo\": " + seqNo + "}}").getBytes(), 0, false);
     }
 
+    public static void sendCmdAck(String vehicleId, int seqNo) throws JsonProcessingException, MqttException {
+        MqttClient mqttClient = MqttConfig.mqttClient;
+        BaseMessage baseMessage = new BaseMessage();
+        RcsAckMessage rcsAckMsg = new RcsAckMessage();
+        rcsAckMsg.SeqNo = seqNo;
+        baseMessage.id = 10010;
+        baseMessage.content = rcsAckMsg;
+        String json = JacksonUtils.toJson(baseMessage);
+        log.info("sendCmd10010: {}", json);
+        mqttClient.publish("/wcs_server/" + vehicleId, json.getBytes(StandardCharsets.UTF_8), 0, false);
+    }
+
 }

servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java

@@ -1,24 +1,19 @@
 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 com.galaxis.rcs.connector.cl2.sendEntity.RcsTaskMessage;
 import lombok.extern.slf4j.Slf4j;
-import org.clever.core.Conv;
 import org.eclipse.paho.mqttv5.common.MqttException;
 
-import java.math.BigDecimal;
-
 /**
  * CL2 车型报文推送
  */
 @Slf4j
 public class Cl2DeviceConnector {
 
-    public void sendTask(String vehicleId, AmrTaskMessage amrTaskMessage) throws MqttException, JsonProcessingException {
-        AmrMessageHandler.sendCmd10010(vehicleId, amrTaskMessage);
+    public void sendTask(String vehicleId, RcsTaskMessage rcsTaskMessage) throws MqttException, JsonProcessingException {
+        AmrMessageHandler.sendCmd10010(vehicleId, rcsTaskMessage);
     }
 
 }

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

@@ -0,0 +1,6 @@
+package com.galaxis.rcs.connector.cl2.sendEntity;
+
+public class RcsAckMessage {
+    // 作业序号 UInt32 所要应答消息的SeqNo(对方的)
+    public int SeqNo;
+}

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

@@ -2,12 +2,11 @@ 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 {
+public class RcsTaskMessage {
     // 作业序号 UInt32
     public int SeqNo;
     // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业）
@@ -82,7 +81,7 @@ public class AmrTaskMessage {
 
     private static final String REDIS_KEY = "amr:task:message:seqNo";
 
-    public AmrTaskMessage() {
+    public RcsTaskMessage() {
         long seqNo = redis.vIncrement(REDIS_KEY);
         if (seqNo > Integer.MAX_VALUE) {
             redis.kDelete(REDIS_KEY);

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

@@ -2,14 +2,13 @@ package com.yvan.logisticsModel;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
-import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage;
+import com.galaxis.rcs.connector.cl2.sendEntity.RcsTaskMessage;
 import lombok.extern.slf4j.Slf4j;
 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 {
@@ -43,7 +42,7 @@ public class PtrAgvConnectorThread extends Thread {
             PtrAgvDeviceTask startTask = null;
             PtrAgvDeviceTask currentTask = null;
             PtrAgvDeviceTask nextTask = null;
-            AmrTaskMessage taskMessage = null;
+            RcsTaskMessage taskMessage = null;
 
             // 计算中的任务
             List<PtrAgvDeviceTask> computingTaskList = new ArrayList<>();
@@ -65,7 +64,7 @@ public class PtrAgvConnectorThread extends Thread {
                 }
 
                 if (startTask == null && !nextTask.isLastTask) {
-                    taskMessage = new AmrTaskMessage();
+                    taskMessage = new RcsTaskMessage();
                     startTask = nextTask;
                     currentTask = nextTask;
                     startTask.seqNo = taskMessage.SeqNo;
@@ -79,7 +78,7 @@ public class PtrAgvConnectorThread extends Thread {
 
                 if (currentTask == nextTask && taskMessage != null) {
                     currentTask.seqNo = taskMessage.SeqNo;
-                    AmrTaskMessage.LinkData link = new AmrTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
+                    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]);

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

@@ -55,9 +55,9 @@ public class PtrAgvItem extends ExecutorItem {
     // agv当前z坐标
     public double z;
     // 当前所在站点的逻辑X坐标 Int32
-    public int logicX = 13;
+    public int logicX;
     // 当前所在站点的逻辑Y坐标 Int32
-    public int logicY = 10;
+    public int logicY;
     // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
     public short direction;
     // agv当前转动角度值
@@ -176,7 +176,7 @@ public class PtrAgvItem extends ExecutorItem {
         for (RcsTaskPlan plan : sequence.taskList) {
             String endPointId = plan.getTargetId();
 
-            if (plan.getPlanType().equals(PlanTaskType.MOVE.toString())) {
+            if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) {
                 // 获取目标点信息
                 StaticItem pointItem = runtime.getStaticItemById(endPointId);
                 linkStore = (List<Map<String, Object>>) pointItem.dt.get("linkStore");

servo/src/main/java/com/yvan/workbench/controller/RcsController.java

@@ -122,6 +122,12 @@ public class RcsController {
 
         logisticsRuntime.pathPlannerMap.get(executorItem.getT())
             .planMoveTask(planSequence, fromItem.getId(), fromDirection, moveTask);
+
+        PtrAgvItem agvItem = (PtrAgvItem) executorItem;
+        agvItem.logicX = fromItem.logicX;
+        agvItem.logicY = fromItem.logicY;
+        agvItem.appendSequence(planSequence);
+
         return Model.newSuccess(planSequence.toPrettyMap());
     }