cl2 接入amr指令

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

@@ -31,7 +31,7 @@ public class RCS {
 
     @SneakyThrows
     static void init() {
-        String fs = Joiner.on("\n").join(FileUtils.readLines(new File("./yvan-lcc/src/example/example1.json"), StandardCharsets.UTF_8));
+        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);
 
         LogisticsRuntimeService.createEnv(1);

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

@@ -1,17 +1,17 @@
 package com.galaxis.rcs.communication;
 
+import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.core.type.TypeReference;
 import com.fasterxml.jackson.databind.JavaType;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import lombok.extern.slf4j.Slf4j;
 
-import java.lang.reflect.Type;
 import java.util.List;
 import java.util.Map;
 import java.util.concurrent.ConcurrentHashMap;
 
 @Slf4j
-public class JacksonGenericUtils {
+public class JacksonUtils {
     private static final ObjectMapper mapper = new ObjectMapper();
     private static final Map<String, JavaType> typeCache = new ConcurrentHashMap<>();
 
@@ -54,6 +54,10 @@ public class JacksonGenericUtils {
         }
     }
 
+    public static String toJson(Object obj) throws JsonProcessingException {
+        return mapper.writeValueAsString(obj);
+    }
+
     private static String buildCacheKey(Class<?> rawType, Class<?>... parameterClasses) {
         StringBuilder key = new StringBuilder(rawType.getName());
         for (Class<?> clazz : parameterClasses) {
@@ -61,4 +65,6 @@ public class JacksonGenericUtils {
         }
         return key.toString();
     }
+
+
 }

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

@@ -1,20 +1,26 @@
 package com.galaxis.rcs.communication.amrCommunication;
 
 
+import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.core.type.TypeReference;
-import com.galaxis.rcs.communication.JacksonGenericUtils;
+import com.galaxis.rcs.communication.JacksonUtils;
 import com.galaxis.rcs.connector.cl2.receiveEntity.*;
 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 lombok.extern.slf4j.Slf4j;
 import org.clever.core.AppContextHolder;
 import org.clever.core.json.JsonWrapper;
 import org.clever.data.redis.Redis;
+import org.eclipse.paho.mqttv5.client.MqttClient;
+import org.eclipse.paho.mqttv5.common.MqttException;
 import org.eclipse.paho.mqttv5.common.MqttMessage;
+import org.springframework.beans.factory.annotation.Autowired;
 import org.springframework.stereotype.Service;
 
+import java.nio.charset.Charset;
 import java.nio.charset.StandardCharsets;
 import java.util.concurrent.ConcurrentHashMap;
 
@@ -73,7 +79,7 @@ public class AmrMessageHandler {
         AmrMessage<?> amrMessage = null;
         switch (id) {
             case 20010:
-                AmrMessage<AmrTaskCompletedMessage> taskCompletedMessage = JacksonGenericUtils.parse(json, typeRef20010Message);
+                AmrMessage<AmrTaskCompletedMessage> taskCompletedMessage = JacksonUtils.parse(json, typeRef20010Message);
                 break;
             case 20011: {
                 int EventId = jw.asInt("content", "EventId");
@@ -84,35 +90,47 @@ public class AmrMessageHandler {
                     case 5:
                     case 6:
                     case 7:
-                        amrMessage = JacksonGenericUtils.parse(json, typeRef20011_defaultMessage);
+                        AmrMessage<AmrTaskStatusMessage<TaskStatusChangeData>> taskStatusChangeDefault = JacksonUtils.parse(json, typeRef20011_defaultMessage);
+
                         break;
                     case 1:
-                        amrMessage = JacksonGenericUtils.parse(json, typeRef20011_1Message);
+                        AmrMessage<AmrTaskStatusMessage<TaskModeChangeData>> taskModeChange = JacksonUtils.parse(json, typeRef20011_1Message);
                         break;
                     case 4:
-                        amrMessage = JacksonGenericUtils.parse(json, typeRef20011_4Message);
+                        AmrMessage<AmrTaskStatusMessage<TaskCompletedData>> taskCompleted = JacksonUtils.parse(json, typeRef20011_4Message);
+                        agvStatusAndInfo.logicX = taskCompleted.content.Info.CurLogicX;
+                        agvStatusAndInfo.logicY = taskCompleted.content.Info.CurLogicY;
+//                agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
+                        agvStatusAndInfo.direction = taskCompleted.content.Info.CurDirection;
+                        log.info("1-Received message: " + json);
                         break;
                     case 8:
-                        amrMessage = JacksonGenericUtils.parse(json, typeRef20011_8Message);
+                        AmrMessage<AmrTaskStatusMessage<TaskTypeChangeData>> taskTypeChange = JacksonUtils.parse(json, typeRef20011_8Message);
                     default:
                         break;
                 }
                 break;
             }
             case 20012:
-                amrMessage = JacksonGenericUtils.parse(json, typeRef20012Message);
+                amrMessage = JacksonUtils.parse(json, typeRef20012Message);
                 break;
             case 20020:
-                AmrMessage<AmrLandmarkMessage> landmarkMessage = JacksonGenericUtils.parse(json, typeRef20020Message);
+                AmrMessage<AmrLandmarkMessage> landmarkMessage = JacksonUtils.parse(json, typeRef20020Message);
+                agvStatusAndInfo.x = landmarkMessage.content.X;
+                agvStatusAndInfo.y = landmarkMessage.content.Y;
+                agvStatusAndInfo.logicX = landmarkMessage.content.CurLogicX;
+                agvStatusAndInfo.logicY = landmarkMessage.content.CurLogicY;
+//                agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
+                agvStatusAndInfo.direction = landmarkMessage.content.CurDirection;
 //                if (landmarkMessage.content.VehicleId == 32) {
-                    log.info("Received message: " + json);
+                    log.info("2-Received message: " + json);
 //                }
                 break;
             case 20050:
-                amrMessage = JacksonGenericUtils.parse(json, typeRef20050Message);
+                amrMessage = JacksonUtils.parse(json, typeRef20050Message);
                 break;
             case 20060:
-                AmrMessage<AmrStatusMessage> statusMessage = JacksonGenericUtils.parse(json, typeRef20060Message);
+                AmrMessage<AmrStatusMessage> statusMessage = JacksonUtils.parse(json, typeRef20060Message);
                 agvStatusAndInfo.agvSOC = statusMessage.content.CurBattery.SOC;
                 agvStatusAndInfo.agvBatteryVoltage = statusMessage.content.CurBattery.Voltage;
 //                agvStatusAndInfo.agvChargingStatus = statusMessage.content.CurBattery.ChargingStatus;
@@ -122,12 +140,14 @@ public class AmrMessageHandler {
                 agvStatusAndInfo.x = statusMessage.content.X;
                 agvStatusAndInfo.y = statusMessage.content.Y;
                 agvStatusAndInfo.orientation = statusMessage.content.CurOrientation;
+                agvStatusAndInfo.logicX = statusMessage.content.CurLogicX;
+                agvStatusAndInfo.logicY = statusMessage.content.CurLogicY;
 //                if (statusMessage.content.VehicleId == 32) {
 //                        log.info("Received message: " + json);
 //                    }
                 break;
             case 20100:
-                AmrMessage<AmrHeartbeatMessage> heartbeatMessage = JacksonGenericUtils.parse(json, typeRef20100Message);
+                AmrMessage<AmrHeartbeatMessage> heartbeatMessage = JacksonUtils.parse(json, typeRef20100Message);
 
 //                if (amrMessage.content instanceof AmrHeartbeatMessage) {
 //                    AmrHeartbeatMessage msg = (AmrHeartbeatMessage) amrMessage.content;
@@ -138,22 +158,22 @@ public class AmrMessageHandler {
 
                 break;
             case 20147:
-                amrMessage = JacksonGenericUtils.parse(json, typeRef20147Message);
+                amrMessage = JacksonUtils.parse(json, typeRef20147Message);
                 break;
             case 20148:
-                amrMessage = JacksonGenericUtils.parse(json, typeRef20148Message);
+                amrMessage = JacksonUtils.parse(json, typeRef20148Message);
                 break;
             case 20149:
-                amrMessage = JacksonGenericUtils.parse(json, typeRef20149Message);
+                amrMessage = JacksonUtils.parse(json, typeRef20149Message);
                 break;
             case 20150:
-                amrMessage = JacksonGenericUtils.parse(json, typeRef20150Message);
+                amrMessage = JacksonUtils.parse(json, typeRef20150Message);
                 break;
             case 20200:
-                amrMessage = JacksonGenericUtils.parse(json, typeRef20200Message);
+                amrMessage = JacksonUtils.parse(json, typeRef20200Message);
                 break;
             case 20250:
-                amrMessage = JacksonGenericUtils.parse(json, typeRef20250Message);
+                amrMessage = JacksonUtils.parse(json, typeRef20250Message);
                 break;
             default:
                 log.error("未知消息：{}", json);
@@ -161,4 +181,13 @@ public class AmrMessageHandler {
         }
     }
 
+    public static AmrStatusAndInfo getAmrStatusAndInfo(int vehicleId) {
+        return agvStatusMap.get(vehicleId);
+    }
+    public static void sendCmd10010(int 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);
+    }
+
 }

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

@@ -34,7 +34,13 @@ public class AmrStatusAndInfo {
     public double y;
     // agv当前z坐标
     public double z;
-    // agv当前方向
+    // 当前所在站点的逻辑X坐标 Int32
+    public int logicX;
+    // 当前所在站点的逻辑Y坐标 Int32
+    public int logicY;
+    // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public short direction;
+    // agv当前转动角度值
     public double orientation;
 
     public AmrStatusAndInfo(String agvId) {

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

@@ -1,11 +1,17 @@
 package com.galaxis.rcs.connector.cl2;
 
 import com.galaxis.rcs.common.entity.RcsTaskPlan;
+import com.galaxis.rcs.communication.amrCommunication.AmrMessageHandler;
+import com.galaxis.rcs.communication.amrCommunication.AmrStatusAndInfo;
+import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage;
 import com.yvan.logisticsModel.ExecutorItem;
 import com.yvan.logisticsModel.FlowItem;
 import com.yvan.logisticsModel.LogisticsRuntime;
 import com.yvan.logisticsModel.StaticItem;
 import lombok.extern.slf4j.Slf4j;
+import org.clever.core.Conv;
+
+import java.math.BigDecimal;
 
 /**
  * CL2 车型报文推送
@@ -15,16 +21,53 @@ public class Cl2DeviceConnector extends Cl2Connector {
 
     @Override
     public void robotMove(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String endPointId) {
+
+        double speed = 1000;
+        // 获取车辆ID
+        int vehicleId = Conv.asInteger(task.getExecutorId());
+        // 获取车辆当前信息
+        AmrStatusAndInfo agvInfo = AmrMessageHandler.getAmrStatusAndInfo(vehicleId);
+
+        // 获取目标点信息
+        StaticItem pointItem = runtime.getStaticItemById(endPointId);
+
+
+        // 生成移动报文
+        AmrTaskMessage message = new AmrTaskMessage();
+        message.SeqNo = 1;
+        message.OperationType = 0;
+        message.GoNow = true;
+        message.StartX = agvInfo.logicX;
+        message.StartY = agvInfo.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 (agvInfo.direction == targetDirection) {
+
+        }
+
+
+
         log.info("Cl2DeviceConnector robotMove: executorItem={}, task={}, endPointId={}",
             executorItem.getId(), task.getPlanTaskId(), endPointId);
 
         // 获取静态数据（货架、地标等）
-        StaticItem pointItem = runtime.getStaticItemById(endPointId);
+
-        log.info("{}", pointItem);
+//        log.info("{}", pointItem);
 
         // 获取车的数据
-        ExecutorItem agv = runtime.executorItemMap.get(executorItem.getId());
+//        ExecutorItem agv = runtime.executorItemMap.get(executorItem.getId());
-        log.info("{}", agv);
+//        log.info("{}", agv);
 
         // 获取箱子的数据
         FlowItem box = runtime.flowItemMap.get("pallet1122");