ddd

servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java

@@ -422,7 +422,7 @@ public class AmrMessageHandler {
 
 
     private void handleTaskCompletedMessage(PtrAgvItem agvItem, AmrTaskCompletedMessage message) {
-        agvItem.updateTask(message.CurX, message.CurY, message.CurDirection, 4);
+        agvItem.taskCompleted(message.CurX, message.CurY, message.CurDirection, 4);
     }
 
     private void handleLandmarkMessage(PtrAgvItem agvItem, AmrLandmarkMessage message) {
@@ -494,7 +494,7 @@ public class AmrMessageHandler {
                 // agvItem.logicY = completedMessage.Info.CurLogicY;
                 // // agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
                 // agvItem.direction = taskCompleted.content.Info.CurDirection;
-                agvItem.updateTask(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection, 4);
+//                agvItem.updateTask(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection, 4);
                 agvItem.updatePosition(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection);
                 break;
             case 8:

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

@@ -241,7 +241,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
         return true;
     }
 
-    public void updateTask(int logicX, int logicY, short direction, int taskStatus) {
+    public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) {
 
         updatePosition(logicX, logicY, direction);
         // 查找当前分组任务
@@ -257,11 +257,12 @@ public abstract class PtrAgvItem extends ExecutorItem {
             }
         }
 
-        if (planTaskSequence.isAllCompleted()) {
+        if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
             fireEvent(AgvEventType.PLAN_COMPLETE, this);
             this.runningDeviceTaskList.clear();
             planTaskSequence = null;
         }
+        LockSupport.unpark(connectorThread);
     }
 
     public void updatePosition(int logicX, int logicY, short direction) {
@@ -339,10 +340,14 @@ public abstract class PtrAgvItem extends ExecutorItem {
             )
         });
 
-        if (needCompute) {
+        if (needCompute && this.runningDeviceTaskList.size() > 0) {
+            int index = this.runningDeviceTaskList.size() - 1;
+            PtrAgvDeviceTask task = this.runningDeviceTaskList.get(index);
+            if (task.groupEndPoint != task.endPoint) {
                 LockSupport.unpark(connectorThread);
             }
         }
+    }
 
     /**
      * 更新设备任务状态 暂时没有处理任务取消相关的状态
@@ -489,18 +494,22 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 linkStore = (List<Map<String, Object>>) pointItem.dt.get("linkStore");
                 int d = -1;
                 if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) {
-                    d = pointItem.logicY > startPoint.logicY ? CDirection.db : CDirection.dt;
+                    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;
+                    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 if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) {
+                    d = direction;
+                    // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
+
                 } else {
                     throw new RuntimeException("无法识别的点位关系");
                 }
@@ -549,6 +558,25 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
             } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) {
 
+                if (deviceTaskList.isEmpty()) {
+                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
+                }
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.transplantLoadAndUnload;
                 deviceTask.pickMode = CPickMode.load;
@@ -594,6 +622,25 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 groupStartPoint = deviceTask.endPoint;
 
             } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
+                if (deviceTaskList.isEmpty()) {
+                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
+                }
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.transplantLoadAndUnload;
                 deviceTask.pickMode = CPickMode.unload;
@@ -638,6 +685,24 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 groupStartPoint = deviceTask.endPoint;
 
             } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
+                if (deviceTaskList.isEmpty()) {
+                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                }
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.charge;
                 deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
@@ -657,7 +722,9 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
         }
 
-
+        if (deviceTaskList.size() <= 0) {
+            return;
+        }
 
         // 标记任务分组结束
         PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);