冻结当前版本，后期可能会修改回滚

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

@@ -13,6 +13,7 @@ import java.util.HashMap;
 import java.util.Iterator;
 import java.util.List;
 import java.util.Objects;
+import java.util.Set;
 import java.util.concurrent.ExecutorService;
 import java.util.concurrent.Executors;
 import java.util.concurrent.LinkedBlockingQueue;
@@ -24,6 +25,7 @@ import org.opentcs.common.LoopbackAdapterConstants;
 import org.opentcs.customizations.kernel.KernelExecutor;
 import org.opentcs.data.model.Point;
 import org.opentcs.data.model.Pose;
+import org.opentcs.data.model.TCSResourceReference;
 import org.opentcs.data.model.Triple;
 import org.opentcs.data.model.Vehicle;
 import org.opentcs.data.order.Route.Step;
@@ -309,6 +311,8 @@ public class LoopbackCommunicationAdapter
     //检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。
     if (!getProcessModel().isSingleStepModeEnabled()
         && !isSimulationRunning) {
+      System.out.println("sendCommand-----====123321");
+
       isSimulationRunning = true;
 
       if (uniqueOrderName == null || !uniqueOrderName.equals(orderName)) {
@@ -500,8 +504,8 @@ public class LoopbackCommunicationAdapter
 
   @Override
   protected synchronized void connectVehicle() {
-    getProcessModel().setCommAdapterConnected(true);
-//    initAGV();
+//    getProcessModel().setCommAdapterConnected(true);
+    initAGV();
   }
 
   @Override
@@ -553,24 +557,13 @@ public class LoopbackCommunicationAdapter
     Step step = command.getStep();
     getProcessModel().setState(Vehicle.State.EXECUTING);
 
-
-    //阻塞当前线程至0xAF订阅结束 todo 为了避免并发可能要注释
-    while (true) {
-      Date now = new Date();
-      if ((now.getTime() - sub0xafDeadline) >= 200) {
-        System.out.println("-startVehicle 0xAF subscription is over");
-        break;
-      }
-
-      try {
-        //AGV运行中，阻塞500ms后轮询
-        System.out.println("-startVehicle 0xAF subscription is not over, Thread sleep 500ms");
-        Thread.sleep(500);
-      } catch (InterruptedException e) {
-        // 处理中断异常，重置中断状态
-        Thread.currentThread().interrupt();
-      }
+    //设置车辆占用
+//    vehicle.withAllocatedResources();
+    List<Set<TCSResourceReference<?>>> allocatedResources = vehicle.getAllocatedResources();
+    for (Set<TCSResourceReference<?>> allocatedResource : allocatedResources) {
+      System.out.println("-startVehicle allocatedResource" + allocatedResource);
     }
+//    vehicle.withClaimedResources()
 
     if (step.getPath() == null) {
       System.out.println("-startVehicle operation...");
@@ -618,6 +611,7 @@ public class LoopbackCommunicationAdapter
 
     //AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞
     HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation);
+    System.out.println("-Starting movement ...");
   }
 
   private void startVehicleSimulation(MovementCommand command) {
@@ -673,11 +667,13 @@ public class LoopbackCommunicationAdapter
     String currentPoint = null;
     long specifyTheTime = SubRobotStatue.intervalTime * 2 + 200;
     Date now = new Date();
-    if ((now.getTime() - AFLastReportTime) <= specifyTheTime) {
+    if ((now.getTime() - AFLastReportTime) >= specifyTheTime && isVehicleConnected()) {
+      //默认订阅结束
       QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
       currentPoint = (qryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString();
       initVehiclePosition(currentPoint);
     } else {
+      //订阅未结束
       currentPoint = getProcessModel().getPosition();
     }
 
@@ -766,6 +762,7 @@ public class LoopbackCommunicationAdapter
       //AGV运行中不能执行动作进入阻塞状态！！！
       QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
       if (qryRobotStatusRsp.runningStatusInfo.agvStatus == 0) {
+        System.out.println();
         System.out.println("-operationExec AGV exec operation");
         break;
       }
@@ -927,13 +924,20 @@ public class LoopbackCommunicationAdapter
    */
   private void finishMoveCmd(MovementCommand command) {
     //检查已发送命令队列的大小是否小于等于1，且未发送命令队列是否为空。
+    System.out.println("-finishMoveCmd getSentCommands size:" + getSentCommands().size());
     if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) {
       System.out.println("-getSentCommands <= 1 && getUnsentCommands is null");
       getProcessModel().setState(Vehicle.State.IDLE);
       //清除订单对应唯一ID
       HybridNavigation.delUniqueOrderID(command);
+      //任务执行结束，开启订阅
+      SUBSCRIBE_STATUS = true;
+      sub0xAF();
+
+      isSimulationRunning = false;
 
       LOG.debug("-callback wms");
+      //订单结束调用wms接口
     }
 
     //如果传入指令和移动指令队列第一条数据相同
@@ -941,7 +945,6 @@ public class LoopbackCommunicationAdapter
       // 是，完成当前任务
       getProcessModel().commandExecuted(getSentCommands().poll());
     } else {
-      //否，
       LOG.warn(
           "-{}: Exec command not oldest in sent queue: {} != {}",
           getName(),
@@ -972,14 +975,10 @@ public class LoopbackCommunicationAdapter
   }
 
   void nextCommand() {
-    if (getSentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) {
+    if (getUnsentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) {
       LOG.debug("Vehicle exec is done.");
       getProcessModel().setState(Vehicle.State.IDLE);
       isSimulationRunning = false;
-
-      //任务执行结束，开启订阅
-      SUBSCRIBE_STATUS = true;
-      sub0xAF();
     }
     else {
       LOG.debug("Triggering exec for next command: {}", getSentCommands().peek());