Merge remote-tracking branch 'origin/master'

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

@@ -30,10 +30,10 @@ 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);
@@ -41,6 +41,12 @@ public class RCS {
             runtime.start();
         }
 
+    }
+
+    public static void ok()
+    {
+    }
+
     /**
      * 添加任务, 示例
      * <pre>

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;

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);

servo/src/main/java/com/galaxis/rcs/connector/ConnectorConsumer.java

@@ -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);
-}

servo/src/main/java/com/galaxis/rcs/connector/ConnectorFactory.java

@@ -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<String, ConnectorConsumer> connectorMap = Maps.newHashMap();
-
-    /**
-     * 虚拟连接器映射
-     */
-    private static final Map<String, ConnectorConsumer> 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;
-    }
-}

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

@@ -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);
-
-}

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 sendTask(String vehicleId, AmrTaskMessage amrTaskMessage) throws MqttException, JsonProcessingException {
-    public void robotMove(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String endPointId) {
+        AmrMessageHandler.sendCmd10010(vehicleId, amrTaskMessage);
-
-        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);
-    }
-
-    @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);
-    }
 }

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 {
+public class VirtualCl2Connector {
-    @Override
-    public void robotMove(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String endWayPoint) {
-        log.info("VirtualCl2Connector robotMove: executorItem={}, task={}, endWayPoint={}",
-            ptrAgvItem.getId(), task.getPlanTaskId(), endWayPoint);
-    }
-
-    @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);
-    }
 }

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 逻辑单位，乘以一定系数才是物理距离

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

@@ -50,4 +50,14 @@ public abstract class BaseItem {
 
         this.dt = (Map<String, Object>) map.get("dt");
     }
+
+    public float getTransformationX() {
+        return tf[0][0];
+    }
+    public float getTransformationY() {
+        return tf[0][2];
+    }
+    public float getTransformationZ() {
+        return tf[0][1];
+    }
 }

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);
         }
     }
 

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 {
+
+}

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.fasterxml.jackson.core.JsonProcessingException;
-import com.galaxis.rcs.common.enums.PlanTaskStatus;
+import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
-import com.galaxis.rcs.connector.ConnectorFactory;
+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);
+
+                if (isPositiveSpeed == (deviceTask.speed > 0) && headerDirection == deviceTask.direction) {
+                    taskCount ++;
+                    distance += euclideanDistance(deviceTask.startPoint.tf[0], deviceTask.endPoint.tf[0]);
+
+                }
+                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);
-                    ConnectorFactory.getConnector(this.executorItem.getT())
+                    } catch (MqttException | JsonProcessingException e) {
-                        .consume(this.executorItem, this.logisticsRuntime, task);
+                        log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, error={}",
-
+                            ptrAgvItem.getId(), deviceTask.planTaskId, e);
-                } catch (Exception e) {
+                    }
-                    log.error("Error processing task: " + task, e);
-                    task.setPlanTaskErrorInfo(e.getMessage());
-                    task.setPlanTaskStatus(PlanTaskStatus.ERROR.toString());
                 }
             }
         } 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);
+    }
 }

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;
+}

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<RcsTaskPlan> planQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);
 
+    final BlockingQueue<PtrAgvDeviceTask> 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<PtrAgvDeviceTask> 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<String, Object> 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;
+
+    }
+
+
 }