cl2 接入amr指令

servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java

@@ -8,5 +8,7 @@ public enum PlanTaskType {
     ROTATION,    // 旋转任务
     LOAD, // 取货任务
     UNLOAD, // 装载任务
-    CHARGE       // 充电任务
+    CHARGE,       // 充电任务
+    FINISH       // 完成任务
+
 }

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

@@ -11,6 +11,7 @@ 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.BaseMessage;
 import com.yvan.logisticsModel.LogisticsRuntimeService;
 import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.extern.slf4j.Slf4j;
@@ -196,8 +197,12 @@ public class AmrMessageHandler {
         return (PtrAgvItem) executorItem;
     }
     public static void sendCmd10010(String vehicleId, AmrTaskMessage amrTaskMessage) throws JsonProcessingException, MqttException {
-        MqttClient mqttClient = AppContextHolder.getBean(MqttClient.class, true);
-        String json = JacksonUtils.toJson(amrTaskMessage);
+        MqttClient mqttClient = MqttConfig.mqttClient;
+        BaseMessage baseMessage = new BaseMessage();
+        baseMessage.id = 10010;
+        baseMessage.content = amrTaskMessage;
+        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/communication/amrCommunication/MqttConfig.java

@@ -13,6 +13,7 @@ import org.springframework.context.annotation.Configuration;
 @Slf4j
 @Configuration
 public class MqttConfig {
+    public static MqttClient mqttClient = null;
     @Value("${mqtt.broker-url}")
     private String brokerUrl;
 
@@ -36,7 +37,7 @@ public class MqttConfig {
     }
 
     @Autowired private AmrMessageHandler amrMessageHandler;
-    @Bean(destroyMethod = "disconnect")
+    @Bean(destroyMethod = "disconnect", name = "mqttClient")
     public MqttClient mqttClient() throws MqttException {
         MqttClient client = new MqttClient(brokerUrl, clientId);
         client.setCallback(new MqttCallback() {
@@ -79,7 +80,7 @@ public class MqttConfig {
         });
         client.connect(mqttConnectOptions());
         client.subscribe("/agv_robot/status", 0);
-
+        mqttClient = client;
         return client;
     }
 }

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

@@ -31,7 +31,7 @@ public class AmrTaskMessage {
     // 路径分段数	UInt8
     public short LinkCounts;
     // 路径分段信息
-    public List<Link> Links;
+    public List<LinkData> Link;
     // AMR内置货位ID（仅对多层移栽有意义）UInt8 1～N
     public short BuiltinSlotNo;
     // 提升移栽货物拣货模式	UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
@@ -92,7 +92,7 @@ public class AmrTaskMessage {
     }
 
     @AllArgsConstructor
-    public static class Link {
+    public static class LinkData {
         //该段目标点X坐标	UInt16 逻辑单位，乘以一定系数才是物理距离
         public int X;
         //该段目标点Y坐标	UInt16 逻辑单位，乘以一定系数才是物理距离

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

@@ -0,0 +1,6 @@
+package com.galaxis.rcs.connector.cl2.sendEntity;
+
+public class BaseMessage {
+    public int id;
+    public Object content;
+}

servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java

@@ -90,7 +90,7 @@ public class PlanTaskSequence {
 
     // 添加完成动作
     public RcsTaskPlan addFinish() {
-        RcsTaskPlan task = this.createTaskPlanEntity(PlanTaskType.ROTATION.toString());
+        RcsTaskPlan task = this.createTaskPlanEntity(PlanTaskType.FINISH.toString());
         this.isFinished = true;
         return task;
     }

servo/src/main/java/com/galaxis/rcs/task/TaskService.java

@@ -50,7 +50,7 @@ public class TaskService {
      * 获取当前等待执行的任务列表
      */
     public List<RcsTaskBiz> getWaitingTaskList(List<ExecutorItem> executorList) {
-        return waitingTaskList;
+        return Lists.newArrayList(waitingTaskList);
     }
 
     /**

servo/src/main/java/com/galaxis/rcs/task/dispatcher/TaiWanDispatcher.java

@@ -13,6 +13,9 @@ public class TaiWanDispatcher {
 
     public void dispatchTask(TaskDispatchFactory factory, List<ExecutorItem> executorList, List<RcsTaskBiz> tasks) {
         // 台湾展会的任务分配逻辑
+        if(tasks == null || executorList == null){
+            return;
+        }
         for (ExecutorItem agv : executorList) {
             for (RcsTaskBiz task : tasks) {
                 if (Objects.equals(agv.getId(), task.getAllocatedExecutorId())) {

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

@@ -96,11 +96,11 @@ public class LogisticsRuntime {
     /**
      * 根据 ID 获取静态物品（如路标点、货架、地堆位等）
      */
-    public StaticItem getStaticItemByLogicXY(int logicX, int logicZ) {
+    public StaticItem getStaticItemByLogicXY(int logicX, int logicY) {
         // 到所有楼层寻找这个物品
         for (Floor floor : this.floorMap.values()) {
             for (StaticItem item : floor.itemMap.values()) {
-                if (item.logicX == logicX && item.logicZ == logicZ) {
+                if (item.logicX == logicX && item.logicY == logicY) {
                     return item;
                 }
             }

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

@@ -56,14 +56,14 @@ public class PtrAgvConnectorThread extends Thread {
                     taskMessage.GoNow = true;
                     taskMessage.StartX = deviceTask.startPoint.logicX;
                     taskMessage.StartY = deviceTask.startPoint.logicY;
-                    taskMessage.Links = new ArrayList<>();
+                    taskMessage.Link = new ArrayList<>();
 
                 }
 
                 deviceTask.seqNo = taskMessage.SeqNo;
 
-                AmrTaskMessage.Link link = new AmrTaskMessage.Link(deviceTask.endPoint.logicX, deviceTask.endPoint.logicY, deviceTask.speed);
-                taskMessage.Links.add(link);
+                AmrTaskMessage.LinkData link = new AmrTaskMessage.LinkData(deviceTask.endPoint.logicX, deviceTask.endPoint.logicY, deviceTask.speed);
+                taskMessage.Link.add(link);
 
                 if (isPositiveSpeed == (deviceTask.speed > 0) && headerDirection == deviceTask.direction) {
                     taskCount ++;
@@ -78,13 +78,14 @@ public class PtrAgvConnectorThread extends Thread {
                     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);
                     }
+                    taskMessage = null;
                 }
             }
         } catch (InterruptedException e) {

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

@@ -49,9 +49,9 @@ public class PtrAgvItem extends ExecutorItem {
     // agv当前z坐标
     public double z;
     // 当前所在站点的逻辑X坐标 Int32
-    public int logicX;
+    public int logicX = 13;
     // 当前所在站点的逻辑Y坐标 Int32
-    public int logicY;
+    public int logicY = 10;
     // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
     public short direction;
     // agv当前转动角度值
@@ -70,6 +70,12 @@ public class PtrAgvItem extends ExecutorItem {
      * 连接器线程
      */
     private final PtrAgvConnectorThread connectorThread;
+
+    public void mapReady() {
+        this.isMapReady = true;
+        this.startConnector();
+    }
+
     /**
      * 启动连接器线程
      */