点到点执行动作

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

@@ -7,7 +7,6 @@ import static java.util.Objects.requireNonNull;
 import com.google.inject.assistedinject.Assisted;
 import jakarta.inject.Inject;
 import java.beans.PropertyChangeEvent;
-import java.time.LocalDateTime;
 import java.util.Arrays;
 import java.util.Date;
 import java.util.HashMap;
@@ -144,6 +143,11 @@ public class LoopbackCommunicationAdapter
    * 最后经过点位
    */
   private static String LAST_PASSED_POINT;
+  /**
+   * 唯一订单name.
+   * 用于判断是否切换执行订单
+   */
+  private static String uniqueOrderName;
 
   /**
    * Creates a new instance.
@@ -316,8 +320,6 @@ public class LoopbackCommunicationAdapter
 
     System.out.println("send cmd print end");
 
-    //订阅0xAF
-//    sub0xAF();
     //AGV控制器执行命令
     HybridNavigation.command(orderName, sourcePoint, destinationPointName, operation);
 
@@ -326,13 +328,12 @@ public class LoopbackCommunicationAdapter
         && !isSimulationRunning) {
       isSimulationRunning = true;
 
-      if (LAST_PASSED_POINT == null) {
-        //设置最后经过点为起点
+      if (uniqueOrderName == null || !uniqueOrderName.equals(orderName)) {
+        //设置唯一订单名称
+        uniqueOrderName = orderName;
+        //记录订单起点
         LAST_PASSED_POINT = sourcePoint;
-        //设置订阅状态为true
-        SUBSCRIBE_STATUS = true;
       }
-      sub0xAF();
 
       // 展示模拟车辆
       if (getSentCommands().isEmpty()) {
@@ -393,7 +394,12 @@ public class LoopbackCommunicationAdapter
         String vehicleNowPosition = getProcessModel().getPosition();
         String lastPassPointId = (queryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString();
         if (vehicleNowPosition == null || !vehicleNowPosition.equals(lastPassPointId)) {
-          initVehiclePosition(lastPassPointId);
+          if ("0".equals(lastPassPointId)) {
+            //最终经过点为0手动设置当前位置
+            initVehiclePosition("2");
+          } else {
+            initVehiclePosition(lastPassPointId);
+          }
         }
 
         //新：设置车辆姿势。(官方弃用设置车辆精确位置)-------------------目前车辆返回位置为固定值！！！！！
@@ -507,12 +513,12 @@ public class LoopbackCommunicationAdapter
 
   @Override
   protected synchronized void connectVehicle() {
-//    getProcessModel().setCommAdapterConnected(true);
     initAGV();
   }
 
   @Override
   protected synchronized void disconnectVehicle() {
+    SUBSCRIBE_STATUS = false;
     getProcessModel().setCommAdapterConnected(false);
   }
 
@@ -562,6 +568,7 @@ public class LoopbackCommunicationAdapter
     if (step.getPath() == null) {
       LOG.debug("-Starting operation...");
       //动作执行待完成
+//      operationSimulation(command, 1);
       operationExec(command);
     } else {
       getProcessModel().getVelocityController().addWayEntry(
@@ -631,17 +638,18 @@ public class LoopbackCommunicationAdapter
     //获取AGV最终经过点
     String currentPoint = getProcessModel().getPosition() != null ? getProcessModel().getPosition() : "";
 
-    //获取当前路径条目并推进时间步长，检查是否仍处于同一路径条目：
+    //判断当前AGV点位是否和最新点位相同：
     if (currentPoint.equals(LAST_PASSED_POINT)) {
-      //若是，则重新调度当前方法以继续模拟。
+      //若是，则重新调度当前方法以继续循环。
       getExecutor().schedule(
           () -> movementExec(command),
           SIMULATION_PERIOD,
           TimeUnit.MILLISECONDS
       );
     } else {
+      //若否，更新当前车辆位置，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。
       LAST_PASSED_POINT = currentPoint;
-      //若否，更新车辆位置为上一路径条目的目标点，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。
+
       LOG.debug("-Movement finished.");
       if (!command.hasEmptyOperation()) {
         //执行AGV动作
@@ -868,8 +876,6 @@ public class LoopbackCommunicationAdapter
       LOG.debug("Vehicle exec is done.");
       getProcessModel().setState(Vehicle.State.IDLE);
       isSimulationRunning = false;
-      LAST_PASSED_POINT = null;
-      SUBSCRIBE_STATUS = false;
     }
     else {
       LOG.debug("Triggering exec for next command: {}", getSentCommands().peek());
@@ -925,7 +931,8 @@ public class LoopbackCommunicationAdapter
         throw new RuntimeException("initAGV 0xAF response is null");
       }
 
-      //开启0xAF订阅
+      //设置订阅状态为true && 开启0xAF订阅
+      SUBSCRIBE_STATUS = true;
       sub0xAF();
 
 
@@ -945,16 +952,19 @@ public class LoopbackCommunicationAdapter
 
       if (qryRobotRunStatusRsp == null) {
         getProcessModel().setCommAdapterConnected(false);
-        throw new RuntimeException("initAGV 0x17 response is null");
+        throw new RuntimeException("-initAGV 0x17 response is null");
       }
 
       if (qryRobotRunStatusRsp.robotLocalizationState == 0) {
         getProcessModel().setCommAdapterConnected(false);
-        throw new RuntimeException("AGV定位失败，请执行手动定位");
+        throw new RuntimeException("-If the AGV positioning fails, please perform manual positioning");
+      }
+      else if (qryRobotRunStatusRsp.robotLocalizationState == 1) {
+        getProcessModel().setPosition(qryRobotRunStatusRsp.currentTargetId.toString());
       }
       else if (qryRobotRunStatusRsp.robotLocalizationState == 2) {
         getProcessModel().setCommAdapterConnected(false);
-        throw new RuntimeException("AGV定位中,请稍后再试");
+        throw new RuntimeException("-agv positioning please try again later");
       }
 
       //5 切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）；
@@ -967,7 +977,7 @@ public class LoopbackCommunicationAdapter
       getProcessModel().setCommAdapterConnected(true);
     } else {
       //0xAF获取AGV状态
-      System.out.println("=================---initAGV_0xAF");
+      System.out.println("---=-=-=----initAGV_0xAF");
       QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
       if (qryRobotStatusRsp == null) {
         getProcessModel().setCommAdapterConnected(false);
@@ -1031,17 +1041,17 @@ public class LoopbackCommunicationAdapter
   }
 
   private void sub0xAF(){
+    Date now = new Date();
+    sub0xafDeadline = now.getTime() + 10000;
     messageProcessingPool.submit(() -> {
-      Date now = new Date();
-      sub0xafDeadline = now.getTime() + 10000;
       SubRobotStatue.command();
     });
   }
 
   private void sub0xB0(){
+    Date now = new Date();
+    sub0xb0Deadline = now.getTime() + 10000;
     messageProcessingPool.submit(() -> {
-      Date now = new Date();
-      sub0xb0Deadline = now.getTime() + 10000;
       SubCargoStatus.command();
     });
   }