update

opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java

@@ -571,10 +571,10 @@ public class LoopbackCommunicationAdapter
     Step step = command.getStep();
     getProcessModel().setState(Vehicle.State.EXECUTING);
 
+    System.out.println("start one operation");
     if (step.getPath() == null) {
       LOG.debug("-Starting operation...");
       //无可执行路径，检查是否需要执行动作
-//      operationSimulation(command, 1);
       operationExec(command);
     } else {
       getProcessModel().getVelocityController().addWayEntry(
@@ -656,12 +656,14 @@ public class LoopbackCommunicationAdapter
       //若否，更新当前车辆位置，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。
       LAST_PASSED_POINT = currentPoint;
 
-      LOG.debug("-Movement finished.");
+
+      System.out.println("-emptyOperation:" + command.hasEmptyOperation());
       if (!command.hasEmptyOperation()) {
         //执行AGV动作
         LOG.debug("-Starting operation...");
         operationExec(command);
       } else {
+        LOG.debug("-Movement Finishing command.");
         //完成当前命令
         finishMoveCmd(command);
         //执行下一个命令
@@ -724,91 +726,103 @@ public class LoopbackCommunicationAdapter
    */
   private void operationExec(MovementCommand command) {
 
-    //todo-------------------
+    System.out.println("operationExec00000000000000");
 
 
     if (ACTION_EXECUTION_STATUS) {
-      //递归进入
+      System.out.println("operationExec-----------");
 
       //todo 读取动作执行状态
       byte[] value = ReadValue.command();
       int execStatus = value[0];
 
+      System.out.println();
       System.out.println("-execStatus:" + execStatus);
 
       if (execStatus == 1) {
-        //动作执行完成,执行重置任务
+      //递归
+      getExecutor().schedule(
+          this::readActionState,
+          200,
+          TimeUnit.MILLISECONDS
+      );
 
-        resetOperation();
+      LOG.debug("-Operation exec finished.");
 
-        LOG.debug("-Operation exec finished.");
-        ACTION_EXECUTION_STATUS = false;
+      ActImmediately.reset();
 
-        finishMoveCmd(command);
+      //todo 读取动作执行状态
+      getExecutor().schedule(
+          this::readActionState,
+          200,
+          TimeUnit.MILLISECONDS
+      );
+
+      ACTION_EXECUTION_STATUS = false;
+
+      finishMoveCmd(command);
+
+      nextCommand();
       } else {
         LOG.debug("-Operation exec 1111111.");
         //递归
         getExecutor().schedule(
             () -> operationExec(command),
-            SIMULATION_PERIOD,
+            500,
             TimeUnit.MILLISECONDS
         );
       }
     } else {
+      System.out.println("operationExec===========");
       //首次进入
       ACTION_EXECUTION_STATUS = true;
 
       //获取动作
       String operation = command.getOperation();
-      LOG.debug("-Operation exec start.");
+      LOG.debug("-Operation exec start:" + operation);
 
       //下发立即动作指令
       if ("LIFT".equals(operation)) {
 
         LOG.debug("-Operation exec LoadOperation.");
 
-        resetOperation();
-
         float height = 1.1f;
         byte modeOfMovement = 0x01;
 
-        //抬升叉齿指令
-        ActImmediately.command(height, modeOfMovement);
+        allotsOperation(height, modeOfMovement);
 
       } else if ("DECLINE".equals(operation)) {
 
         LOG.debug("-Operation exec UnloadOperation.");
 
-        resetOperation();
 
         float height = 0.1f;
         byte modeOfMovement = 0x02;
 
         //降叉齿指令
-        ActImmediately.command(height, modeOfMovement);
+        allotsOperation(height, modeOfMovement);
 
       } else if (operation.equals(this.getRechargeOperation())) {
 
         LOG.debug("-Operation exec RechargeOperation.");
 
-        resetOperation();
+//        allotsOperation();
 
         //充电指令
 
       } else {
-        LOG.debug("-Operation exec 2222222.");
+        LOG.debug("-Operation exec :" + operation);
 
         //动作未定义，不执行
         ACTION_EXECUTION_STATUS = false;
         nextCommand();
       }
 
-      LOG.debug("-Operation exec 3333333.");
-
+      LOG.debug("-Operation exec 222222.");
       //递归
       getExecutor().schedule(
           () -> operationExec(command),
-          SIMULATION_PERIOD,
+          500,
           TimeUnit.MILLISECONDS
       );
     }
@@ -817,18 +831,44 @@ public class LoopbackCommunicationAdapter
     //todo-------------------
   }
 
-  private void resetOperation() {
+  private void allotsOperation(float height, byte modeOfMovement)  {
+
+    LOG.debug("allotsOperation111111");
+
+    //todo 读取动作执行状态
+    getExecutor().schedule(
+        this::readActionState,
+        200,
+        TimeUnit.MILLISECONDS
+    );
     ActImmediately.reset();
+
+    //todo 读取动作执行状态
+    getExecutor().schedule(
+        this::readActionState,
+        200,
+        TimeUnit.MILLISECONDS
+    );
+    //抬升叉齿指令
+    ActImmediately.command(height, modeOfMovement);
+  }
+
+  /**
+   * 读取动作状态
+   */
+  private void readActionState() {
+
     //todo 读取动作执行状态
     byte[] value = ReadValue.command();
-    int execStatus = value[0];
+    int execCmd = value[0];
 
-    while (execStatus == 2) {
-      LOG.debug("-resetOperation error");
-      byte[] value1 = ReadValue.command();
-      execStatus = value1[0];
+    if (execCmd == 2) {
+      getExecutor().schedule(
+          () -> readActionState(),
+          200,
+          TimeUnit.MILLISECONDS
+      );
     }
-
   }
 
   /**