cl2 接入amr指令

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

@@ -100,6 +100,9 @@ public class RCS {
 
         // 规划任务序列
         PlanTaskSequence planSequence = new PlanTaskSequence(executorId, logisticsRuntime, bizTask, "demo");
+        planSequence.addMoveTo("20");
+        planSequence.addMoveTo("21");
+        planSequence.addMoveTo("22");
         planSequence.addMoveTo("23");
         planSequence.addRotationTo(90);
         planSequence.addMoveTo("24");

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

@@ -37,55 +37,67 @@ public class PtrAgvConnectorThread extends Thread {
 
             float distance = 0;
             int taskCount = 0;
-            Boolean isPositiveSpeed = null;
-            short headerDirection = -1;
-            PtrAgvDeviceTask startTask = null;
 
+            PtrAgvDeviceTask startTask = null;
+            PtrAgvDeviceTask currentTask = null;
+            PtrAgvDeviceTask nextTask = null;
             AmrTaskMessage taskMessage = null;
 
             while (running.get()) {
                 // 从队列中获取任务，如果队列为空则阻塞等待
-                PtrAgvDeviceTask deviceTask = this.executorItem.deviceTaskQueue.take();
+                if (nextTask == null) {
+                    nextTask = this.executorItem.deviceTaskQueue.take();
+                } else {
+                    currentTask = nextTask;
+                }
+
                 if (startTask == null) {
-                    startTask = deviceTask;
-                    isPositiveSpeed = deviceTask.speed > 0;
-                    headerDirection = deviceTask.direction;
                     taskMessage = new AmrTaskMessage();
-                    taskMessage.OperationType = deviceTask.operationType;
+                    startTask = nextTask;
-                    taskMessage.PickMode = deviceTask.pickMode;
+                    currentTask = nextTask;
+                    startTask.seqNo = taskMessage.SeqNo;
+                    taskMessage.OperationType = startTask.operationType;
+                    taskMessage.PickMode = startTask.pickMode;
                     taskMessage.GoNow = true;
-                    taskMessage.StartX = deviceTask.startPoint.logicX;
+                    taskMessage.StartX = startTask.startPoint.logicX;
-                    taskMessage.StartY = deviceTask.startPoint.logicY;
+                    taskMessage.StartY = startTask.startPoint.logicY;
                     taskMessage.Link = new ArrayList<>();
-
                 }
 
-                deviceTask.seqNo = taskMessage.SeqNo;
+                if (currentTask == nextTask) {
-
+                    currentTask.seqNo = taskMessage.SeqNo;
-                AmrTaskMessage.LinkData link = new AmrTaskMessage.LinkData(deviceTask.endPoint.logicX, deviceTask.endPoint.logicY, deviceTask.speed);
+                    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]);
+                }
 
-                if (isPositiveSpeed == (deviceTask.speed > 0) && headerDirection == deviceTask.direction) {
-                    taskCount ++;
-                    distance += euclideanDistance(deviceTask.startPoint.tf[0], deviceTask.endPoint.tf[0]);
 
-                }
+                if (currentTask.operationType > 0 || currentTask.pickMode > 0
-                if((isPositiveSpeed != (deviceTask.speed > 0) || headerDirection != deviceTask.direction)
+                    || (((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction) && taskMessage.Link.size() > 0)
                     // 单向移动距离大于2m时拆分任务指令
                     || (distance > 2 && taskCount > 1)) {
-                    taskMessage.EndX = deviceTask.endPoint.logicX;
+
-                    taskMessage.EndY = deviceTask.endPoint.logicY;
+                    taskMessage.OperationType = currentTask.operationType;
-                    distance = 0;
+                    taskMessage.PickMode = currentTask.pickMode;
-                    taskCount = 0;
+                    taskMessage.EndX = currentTask.endPoint.logicX;
-                    startTask = null;
+                    taskMessage.EndY = currentTask.endPoint.logicY;
 
                     try {
                         cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage);
                     } catch (MqttException | JsonProcessingException e) {
                         log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, error={}",
-                            ptrAgvItem.getId(), deviceTask.planTaskId, e);
+                            ptrAgvItem.getId(), currentTask.planTaskId, e);
                     }
+                    distance = 0;
+                    taskCount = 0;
                     taskMessage = null;
+                    startTask = null;
+                    nextTask = null;
+                }
+
+                if (nextTask == currentTask) {
+                    nextTask = null;
                 }
             }
         } catch (InterruptedException e) {
@@ -110,11 +122,11 @@ public class PtrAgvConnectorThread extends Thread {
         return running.get();
     }
 
-    public static double euclideanDistance(float[] p1, float[] p2) {
+    public static float 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);
+        return (float) Math.sqrt(dx * dx + dy * dy + dz * dz);
     }
 }

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

@@ -6,6 +6,7 @@ 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 org.clever.core.json.JsonWrapper;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -203,6 +204,9 @@ public class PtrAgvItem extends ExecutorItem {
         planQueue.addAll(sequence.taskList);
         deviceTaskQueue.addAll(deviceTaskList);
 
+        String json = JsonWrapper.toJson(deviceTaskList);
+        log.info("deviceTaskList: {}", json);
+
         // TODO: 开启轮询线程，等待下一个待执行任务
     }