update

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

@@ -61,9 +61,9 @@ import org.slf4j.LoggerFactory;
  */
 public class LoopbackCommunicationAdapter
     extends
-      BasicVehicleCommAdapter
+    BasicVehicleCommAdapter
     implements
-      SimVehicleCommAdapter {
+    SimVehicleCommAdapter {
 
   /**
    * The name of the load handling device set by this adapter.
@@ -151,9 +151,13 @@ public class LoopbackCommunicationAdapter
    */
   private static String uniqueOrderName;
   /**
-   * 订单参数
+   * 订阅状态
    */
-  private static HashMap<String, Object> orderParams;
+  private static boolean SUBSCRIBE_STATUS_AF = false;
+  /**
+   * 延迟时间
+   */
+  private static long BLOCKING_TIME = 0;
 
   /**
    * Creates a new instance.
@@ -293,26 +297,23 @@ public class LoopbackCommunicationAdapter
 
     System.out.println(cmd);
 
-    orderParams = new HashMap<>();
+    //停止订阅
+    SUBSCRIBE_STATUS = false;
 
     //订单ID
     String orderName = cmd.getTransportOrder().getName();
-    orderParams.put("orderName", orderName);
 
     //下发起点
     String sourcePointName = null;
     if (cmd.getStep().getSourcePoint() != null) {
       sourcePointName = cmd.getStep().getSourcePoint().getName();
     }
-    orderParams.put("sourcePointName", sourcePointName);
 
     //下发终点
     String destinationPointName = cmd.getStep().getDestinationPoint().getName();
-    orderParams.put("destinationPointName", destinationPointName);
 
     //当前点位操作
     String operation = cmd.getOperation();
-    orderParams.put("operation", operation);
 
     //检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。
     if (!getProcessModel().isSingleStepModeEnabled()
@@ -324,10 +325,6 @@ public class LoopbackCommunicationAdapter
         uniqueOrderName = orderName;
         //记录订单起点---更新复位后可
         LAST_PASSED_POINT = sourcePointName;
-        if (cmd.getStep().getPath() != null) {
-          //AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞（可能会和订阅并发，如果复现需要添加延时）
-          HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation);
-        }
       }
 
       // 展示模拟车辆
@@ -391,7 +388,7 @@ public class LoopbackCommunicationAdapter
         if (vehicleNowPosition == null || !vehicleNowPosition.equals(lastPassPointId)) {
           if ("0".equals(lastPassPointId)) {
             //最终经过点为0手动设置当前位置
-            initVehiclePosition("2");
+            initVehiclePosition("0");
           } else {
             initVehiclePosition(lastPassPointId);
           }
@@ -560,9 +557,9 @@ 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...");
+      System.out.println("-startVehicle operation...");
+      LOG.debug("-startVehicle operation...");
       operationExec(command);
     } else {
       getProcessModel().getVelocityController().addWayEntry(
@@ -574,6 +571,24 @@ public class LoopbackCommunicationAdapter
           )
       );
 
+      //订单ID
+      String orderName = command.getTransportOrder().getName();
+
+      //下发起点
+      String sourcePointName = null;
+      if (command.getStep().getSourcePoint() != null) {
+        sourcePointName = command.getStep().getSourcePoint().getName();
+      }
+
+      //下发终点
+      String destinationPointName = command.getStep().getDestinationPoint().getName();
+
+      //当前点位操作
+      String operation = command.getOperation();
+
+      //AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞（可能会和订阅并发，如果复现需要添加延时）
+      HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation);
+
       LOG.debug("-Starting movement ...");
       movementExec(command);
     }
@@ -630,26 +645,29 @@ public class LoopbackCommunicationAdapter
     }
 
     //获取AGV最终经过点
-    String currentPoint = getProcessModel().getPosition() != null ? getProcessModel().getPosition() : "";
+    System.out.println("--0xAF start");
+    QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
+    System.out.println("--0xAF end");
+    String currentPoint = (qryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString();
+    initVehiclePosition(currentPoint);
 
-    //判断当前AGV点位是否和最新点位相同：
+    //判断当前AGV点位是否和最新点位相同
     if (currentPoint.equals(LAST_PASSED_POINT)) {
       //若是，则重新调度当前方法以继续循环。
       getExecutor().schedule(
           () -> movementExec(command),
-          SIMULATION_PERIOD,
+          500,
           TimeUnit.MILLISECONDS
       );
     } else {
       //若否，更新当前车辆位置，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。
       LAST_PASSED_POINT = currentPoint;
 
-
       System.out.println("-emptyOperation:" + command.hasEmptyOperation());
       if (!command.hasEmptyOperation()) {
         //执行AGV动作
-        System.out.println("-Starting operation...");
+        System.out.println("-movementExec operation...");
-        LOG.debug("-Starting operation...");
+        LOG.debug("-movementExec operation...");
 
         operationExec(command);
       } else {
@@ -714,19 +732,9 @@ public class LoopbackCommunicationAdapter
    *
    * @param command 要执行的命令。
    */
-  private void operationExec(MovementCommand command) {
+  private synchronized void operationExec(MovementCommand command) {
 
-    //关闭订阅
+    System.out.println("-operationExec00000000000000");
-    SUBSCRIBE_STATUS = false;
-
-    try {
-      //订阅概率会和动作并发，所以执行动作前需要关闭订阅并进行线程阻塞
-      Thread.sleep(SubRobotStatue.subDuration);
-    } catch (Exception e) {
-      System.out.println(e.getMessage());
-    }
-
-    System.out.println("operationExec00000000000000");
 
     //获取动作
     String operation = command.getOperation();
@@ -762,18 +770,6 @@ public class LoopbackCommunicationAdapter
       LOG.debug("-NOT EXECUTED OPERATION:" + operation);
     }
 
-    while (ActImmediately.ACTION_EXECUTE_STATE) {
-      try {
-        //订阅概率会和动作并发，所以执行动作前需要关闭订阅并进行线程阻塞
-        Thread.sleep(1000);
-      } catch (Exception e) {
-        System.out.println(e.getMessage());
-      }
-    }
-
-    //开启订阅
-    SUBSCRIBE_STATUS = true;
-    sub0xAF();
     //完成当前命令
     finishMoveCmd(command);
     //执行下一个命令
@@ -915,9 +911,9 @@ public class LoopbackCommunicationAdapter
 
   private void finishMovementCommand(MovementCommand command) {
     //Set the vehicle state to idle
-    if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) {
+//    if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) {
-      getProcessModel().setState(Vehicle.State.IDLE);
+//      getProcessModel().setState(Vehicle.State.IDLE);
-    }
+//    }
     if (Objects.equals(getSentCommands().peek(), command)) {
       // Let the comm adapter know we have finished this command. 让通信适配器知道我们已经完成了这个命令。
       getProcessModel().commandExecuted(getSentCommands().poll());
@@ -938,6 +934,13 @@ public class LoopbackCommunicationAdapter
       LOG.debug("Vehicle exec is done.");
       getProcessModel().setState(Vehicle.State.IDLE);
       isSimulationRunning = false;
+
+      SUBSCRIBE_STATUS_AF = false;
+      BLOCKING_TIME = 0;
+
+      //任务执行结束，手动开启订阅
+      SUBSCRIBE_STATUS = true;
+      sub0xAF();
     }
     else {
       LOG.debug("Triggering exec for next command: {}", getSentCommands().peek());
@@ -994,20 +997,12 @@ public class LoopbackCommunicationAdapter
       }
 
       //设置订阅状态为true && 开启0xAF订阅
-      SUBSCRIBE_STATUS = true;
+//      SUBSCRIBE_STATUS = true;
       sub0xAF();
 
-
       //开启0xB0订阅
 //      sub0xB0()
 
-//      //新：设置车辆姿势。(官方弃用设置车辆精确位置)-------------------目前车辆返回位置为固定值！！！！！
-//      long positionX = (long) qryRobotStatusRsp.locationStatusInfo.globalX;
-//      long positionY = (long) qryRobotStatusRsp.locationStatusInfo.globalY;
-//      Triple triple = new Triple(positionX, positionY, 0);
-//      double positionAngle = qryRobotStatusRsp.locationStatusInfo.absoluteDirecAngle;
-//      getProcessModel().setPose(new Pose(triple, positionAngle));
-
       //3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
       System.out.println("=================---initAGV_0x17");
       QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command();
@@ -1104,7 +1099,7 @@ public class LoopbackCommunicationAdapter
 
   private void sub0xAF(){
     Date now = new Date();
-    sub0xafDeadline = now.getTime() + (SubRobotStatue.intervalTime * 2);
+    sub0xafDeadline = now.getTime() + SubRobotStatue.subDuration;
     messageProcessingPool.submit(() -> {
       SubRobotStatue.command();
     });

opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java

@@ -241,41 +241,66 @@ public class KCCommandDemo {
 //        }
 //      }
 
+//    {
+//
+//      System.out.println("00000");
+//      Thread.sleep(100);
+//      ActImmediately.reset();
+//
+//      //todo 读取动作执行状态
+//      int execStatus = 2;
+//      while (execStatus == 2) {
+//        Thread.sleep(200);
+//        byte[] value = ReadValue.command();
+//        execStatus = value[0];
+//      }
+//
+//      System.out.println("111111111");
+//      Thread.sleep(100);
+//      ActImmediately.reset();
+//
+//      //todo 读取动作执行状态
+//      int execStatus2 = 2;
+//      while (execStatus2 == 2) {
+//        Thread.sleep(200);
+//        byte[] value = ReadValue.command();
+//        execStatus2 = value[0];
+//      }
+//
+//      float height = 1.1f;
+//      byte modeOfMovement = 0x01;
+//
+//      Thread.sleep(100);
+//      //抬升叉齿指令
+//      ActImmediately.command(height, modeOfMovement);
+//
+//      Thread.sleep(100);
+//      ActImmediately.reset();
+//    }
     {
 
-      System.out.println("00000");
+      float height = 1.3f;
-      Thread.sleep(100);
-      ActImmediately.reset();
-
-      //todo 读取动作执行状态
-      int execStatus = 2;
-      while (execStatus == 2) {
-        Thread.sleep(200);
-        byte[] value = ReadValue.command();
-        execStatus = value[0];
-      }
-
-      System.out.println("111111111");
-      Thread.sleep(100);
-      ActImmediately.reset();
-
-      //todo 读取动作执行状态
-      int execStatus2 = 2;
-      while (execStatus2 == 2) {
-        Thread.sleep(200);
-        byte[] value = ReadValue.command();
-        execStatus2 = value[0];
-      }
-
-      float height = 1.1f;
       byte modeOfMovement = 0x01;
+      ActImmediately.allotsOperation(height, modeOfMovement);
 
-      Thread.sleep(100);
+//      //读取动作执行状态
-      //抬升叉齿指令
+//      ActImmediately.readActionState();
-      ActImmediately.command(height, modeOfMovement);
+//      //重置
-
+//      ActImmediately.reset();
-      Thread.sleep(100);
+//      //读取动作执行状态
-      ActImmediately.reset();
+//      ActImmediately.readActionState();
+//      float height = 0.1f;
+//      byte modeOfMovement = 0x02;
+//      //执行叉齿指令
+//      ActImmediately.command(height, modeOfMovement);
+//      //读取动作执行状态
+//      ActImmediately.readActionState();
+//      //重置
+//      ActImmediately.reset();
+//      //读取重置状态
+//      ActImmediately.readActionState();
+//
+//      ActImmediately.ACTION_EXECUTE_STATE = false;
     }
 
 //    {

opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java

@@ -94,10 +94,10 @@ public class ActImmediately extends BaseCommand{
   /**
    * 读取动作状态
    */
-  private static void readActionState() {
+  public static void readActionState() {
 
     try {
-      Thread.sleep(200);
+      Thread.sleep(100);
 
       //todo 读取动作执行状态
       byte[] value = ReadValue.command();
@@ -107,7 +107,7 @@ public class ActImmediately extends BaseCommand{
       System.out.println("-execStatus:" + execStatus);
 
       while (execStatus == 2) {
-        Thread.sleep(200);
+        Thread.sleep(500);
         byte[] value1 = ReadValue.command();
         execStatus = value1[0];
       }
@@ -118,7 +118,7 @@ public class ActImmediately extends BaseCommand{
     }
   }
 
-  public static void allotsOperation(float height, byte modeOfMovement)  {
+  public static void allotsOperation(float height, byte modeOfMovement) {
 
     //读取动作执行状态
     readActionState();
@@ -135,6 +135,6 @@ public class ActImmediately extends BaseCommand{
     //读取重置状态
     readActionState();
 
-    ACTION_EXECUTE_STATE = false;
+//    ACTION_EXECUTE_STATE = false;
   }
 }

opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java

@@ -49,7 +49,7 @@ public class HybridNavigation
   /**
    * 订单映射最大订单ID.
    */
-  private static int currentMaxiOrderId = 1;
+  private static int currentMaxiOrderId = 50;
 
 
   /**

opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubRobotStatue.java

@@ -17,11 +17,11 @@ public class SubRobotStatue
   /**
    * 上报时间间隔
    */
-  public static final int intervalTime = 1000;
+  public static final int intervalTime = 500;
   /**
    * 订阅时长
    */
-  public static int subDuration = intervalTime * 3;
+  public static int subDuration = intervalTime * 5;
 
   /**
    * decs: 下发订阅信息