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

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

@@ -89,6 +89,7 @@ public class AmrMessageHandler {
                 break;
             case 20011: {
                 int EventId = jw.asInt("content", "EventId");
+                log.info("1-Received message: " + json);
                 switch (EventId) {
                     case 0:
                     case 2:
@@ -97,21 +98,27 @@ public class AmrMessageHandler {
                     case 6:
                     case 7:
                         AmrMessage<AmrTaskStatusMessage<TaskStatusChangeData>> taskStatusChangeDefault = JacksonUtils.parse(json, typeRef20011_defaultMessage);
-
+                        AmrTaskStatusMessage<TaskStatusChangeData> statusMessage = taskStatusChangeDefault.content;
+                        agvItem.updateDeviceTaskStatus((int)statusMessage.SeqNo, 0, 0, statusMessage.EventId);
                         break;
                     case 1:
                         AmrMessage<AmrTaskStatusMessage<TaskModeChangeData>> taskModeChange = JacksonUtils.parse(json, typeRef20011_1Message);
+                        AmrTaskStatusMessage<TaskModeChangeData> modeMessage = taskModeChange.content;
+                        agvItem.updateDeviceTaskStatus((int)modeMessage.SeqNo, 0, 0, modeMessage.EventId);
                         break;
                     case 4:
                         AmrMessage<AmrTaskStatusMessage<TaskCompletedData>> taskCompleted = JacksonUtils.parse(json, typeRef20011_4Message);
-                        agvItem.logicX = taskCompleted.content.Info.CurLogicX;
+                        AmrTaskStatusMessage<TaskCompletedData> completedMessage = taskCompleted.content;
-                        agvItem.logicY = taskCompleted.content.Info.CurLogicY;
+                        agvItem.updateDeviceTaskStatus((int)completedMessage.SeqNo, completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, completedMessage.EventId);
+                        agvItem.logicX = completedMessage.Info.CurLogicX;
+                        agvItem.logicY = completedMessage.Info.CurLogicY;
 //                agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
                         agvItem.direction = taskCompleted.content.Info.CurDirection;
-                        log.info("1-Received message: " + json);
                         break;
                     case 8:
                         AmrMessage<AmrTaskStatusMessage<TaskTypeChangeData>> taskTypeChange = JacksonUtils.parse(json, typeRef20011_8Message);
+                        AmrTaskStatusMessage<TaskTypeChangeData> typeMessage = taskTypeChange.content;
+                        agvItem.updateDeviceTaskStatus((int)typeMessage.SeqNo, 0, 0, typeMessage.EventId);
                     default:
                         break;
                 }
@@ -129,7 +136,7 @@ public class AmrMessageHandler {
 //                agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
                 agvItem.direction = landmarkMessage.content.CurDirection;
 //                if (landmarkMessage.content.VehicleId == 32) {
-                    log.info("2-Received message: " + json);
+//                    log.info("2-Received message: " + json);
 //                }
                 break;
             case 20050:

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

@@ -7,22 +7,24 @@ 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 {
     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, Cl2DeviceConnector cl2DeviceConnector, PtrAgvItem ptrAgvItem, LogisticsRuntime logisticsRuntime) {
+
-        super("ExecutorConnector-" + executorItem.getId());
+
-        this.executorItem = executorItem;
+    public PtrAgvConnectorThread(PtrAgvItem ptrAgvItem, Cl2DeviceConnector cl2DeviceConnector, LogisticsRuntime logisticsRuntime) {
+        super("ExecutorConnector-" + ptrAgvItem.getId());
         this.cl2DeviceConnector = cl2DeviceConnector;
         this.ptrAgvItem = ptrAgvItem;
         this.logisticsRuntime = logisticsRuntime;
@@ -31,7 +33,7 @@ public class PtrAgvConnectorThread extends Thread {
     @Override
     public void run() {
         running.set(true);
-        log.info("Connector thread started for executor: {}", this.executorItem.getId());
+        log.info("Connector thread started for executor: {}", this.ptrAgvItem.getId());
 
         try {
 
@@ -43,15 +45,26 @@ public class PtrAgvConnectorThread extends Thread {
             PtrAgvDeviceTask nextTask = null;
             AmrTaskMessage taskMessage = null;
 
+            // 计算中的任务
+            List<PtrAgvDeviceTask> computingTaskList = new ArrayList<>();
             while (running.get()) {
+
+                for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
+                    if (task.taskGroupStatus < 3) {
+                        LockSupport.park(); // 阻塞当前线程
+                        break;
+                    }
+                }
+
                 // 从队列中获取任务，如果队列为空则阻塞等待
                 if (nextTask == null) {
-                    nextTask = this.executorItem.deviceTaskQueue.take();
+                    nextTask = this.ptrAgvItem.deviceTaskQueue.take();
+
                 } else {
                     currentTask = nextTask;
                 }
 
-                if (startTask == null) {
+                if (startTask == null && !nextTask.isLastTask) {
                     taskMessage = new AmrTaskMessage();
                     startTask = nextTask;
                     currentTask = nextTask;
@@ -64,19 +77,23 @@ public class PtrAgvConnectorThread extends Thread {
                     taskMessage.Link = new ArrayList<>();
                 }
 
-                if (currentTask == nextTask) {
+                if (currentTask == nextTask && taskMessage != null) {
                     currentTask.seqNo = taskMessage.SeqNo;
                     AmrTaskMessage.LinkData link = new AmrTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
                     taskMessage.Link.add(link);
                     taskCount++;
                     distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]);
+                    computingTaskList.add(currentTask);
                 }
 
-
+                // 组织设备任务并发送
-                if (currentTask.operationType > 0 || currentTask.pickMode > 0
+                else if (taskMessage != null &&
-                    || (((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction) && taskMessage.Link.size() > 0)
+                    (currentTask.operationType > 0 // 当前任务不是移动任务
-                    // 单向移动距离大于2m时拆分任务指令
+                    || currentTask.pickMode > 0 // 当前作业不是默认作业（无）
-                    || (distance > 2 && taskCount > 1)) {
+                    || nextTask.isLastTask  // 下一个任务是结束任务
+                    || (((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction) && taskMessage.Link.size() > 0) // 下一个任务和开始任务方向不一致
+                    // 单向移动距离大于2m时并且点位数量大于1
+                    || (distance > 2 && taskCount > 1))) {
 
                     taskMessage.OperationType = currentTask.operationType;
                     taskMessage.PickMode = currentTask.pickMode;
@@ -84,7 +101,14 @@ public class PtrAgvConnectorThread extends Thread {
                     taskMessage.EndY = currentTask.endPoint.logicY;
 
                     try {
+                        // 发送任务
                         cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage);
+                        this.ptrAgvItem.runningDeviceTaskList.addAll(computingTaskList);
+                        for (PtrAgvDeviceTask task : computingTaskList) {
+                            task.taskStatus = 1;
+                            task.taskGroupStatus = 1;
+                        }
+                        computingTaskList.clear();
                     } catch (MqttException | JsonProcessingException e) {
                         log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, error={}",
                             ptrAgvItem.getId(), currentTask.planTaskId, e);
@@ -95,14 +119,14 @@ public class PtrAgvConnectorThread extends Thread {
                     startTask = null;
                 }
 
-                if (nextTask == currentTask) {
+                if (nextTask == currentTask || nextTask.isLastTask) {
                     nextTask = null;
                 }
             }
         } catch (InterruptedException e) {
-            System.out.println("Connector thread interrupted for executor: " + this.executorItem.getId());
+            System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId());
         } finally {
-            System.out.println("Connector thread stopped for executor: " + this.executorItem.getId());
+            System.out.println("Connector thread stopped for executor: " + this.ptrAgvItem.getId());
         }
     }
 

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

@@ -24,4 +24,11 @@ public class PtrAgvDeviceTask {
     public Long bizTaskId;
     // 作业序号 发送给小车的作业序号
     public int seqNo;
+
+    // 任务状态 0创建 1发送成功 2接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
+    public int taskStatus = 0;
+    public int taskGroupStatus = 0;
+
+    // 是否最后任务
+    public boolean isLastTask = false;
 }

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

@@ -12,6 +12,7 @@ import java.util.ArrayList;
 import java.util.List;
 import java.util.Map;
 import java.util.concurrent.BlockingQueue;
+import java.util.concurrent.locks.LockSupport;
 
 //0.4m/ss // a  max 1.2m/s
 //90 = 3.5s cl2
@@ -60,6 +61,9 @@ public class PtrAgvItem extends ExecutorItem {
     public double orientation;
 
 
+    // 执行中的任务
+    public List<PtrAgvDeviceTask> runningDeviceTaskList = new ArrayList<>();
+
     /**
      * 当前执行的任务规划列表
      */
@@ -73,6 +77,37 @@ public class PtrAgvItem extends ExecutorItem {
      */
     private final PtrAgvConnectorThread connectorThread;
 
+    /**
+     * 更新设备任务状态 暂时没有处理任务取消相关的状态
+     * @param seqNo
+     * @param x
+     * @param y
+     * @param messageStatus
+     */
+    public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) {
+
+        if (messageStatus < 2) {
+            return;
+        }
+
+        boolean needCompute = true;
+        for (PtrAgvDeviceTask task : runningDeviceTaskList) {
+            if (task.seqNo == seqNo) {
+                task.taskGroupStatus = messageStatus;
+                if (task.x == x && task.y == y) {
+                    task.taskStatus = 4;
+                }
+            }
+            if (task.taskGroupStatus < 3 /*|| task.taskStatus < 4*/) {
+                needCompute = false;
+            }
+        }
+        if (needCompute) {
+            LockSupport.unpark(connectorThread);
+        }
+    }
+
+
     public void mapReady() {
         this.isMapReady = true;
         this.startConnector();
@@ -202,6 +237,10 @@ public class PtrAgvItem extends ExecutorItem {
             }
 
         }
+        // 添加结束任务
+        PtrAgvDeviceTask deviceTaskEnd = new PtrAgvDeviceTask();
+        deviceTaskEnd.isLastTask = true;
+        deviceTaskList.add(deviceTaskEnd);
 
         planQueue.addAll(sequence.taskList);
         deviceTaskQueue.addAll(deviceTaskList);
@@ -218,7 +257,7 @@ public class PtrAgvItem extends ExecutorItem {
 
     public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
         super(logisticsRuntime, raw);
-        this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, this, logisticsRuntime);
+        this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime);
     }