动作相关指令

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

@@ -151,9 +151,9 @@ public class LoopbackCommunicationAdapter
    */
   private static String uniqueOrderName;
   /**
-   * 动作执行状态
+   * 订单参数
    */
-  private static boolean ACTION_EXECUTION_STATUS;
+  private static HashMap<String, Object> orderParams;
 
   /**
    * Creates a new instance.
@@ -293,41 +293,26 @@ public class LoopbackCommunicationAdapter
 
     System.out.println(cmd);
 
-    System.out.println("send cmd print start");
+    orderParams = new HashMap<>();
-//    SubRobotStatue.command();
 
     //订单ID
     String orderName = cmd.getTransportOrder().getName();
-    System.out.println("orderID:" + orderName);
+    orderParams.put("orderName", orderName);
 
-    //路线名称
+    //下发起点
-    String pathName = cmd.getStep().getPath().getName();
+    String sourcePointName = null;
-    System.out.println("pathName:" + pathName);
+    if (cmd.getStep().getSourcePoint() != null) {
+      sourcePointName = cmd.getStep().getSourcePoint().getName();
+    }
+    orderParams.put("sourcePointName", sourcePointName);
 
-    //路线长度
+    //下发终点
-    long length = cmd.getStep().getPath().getLength();
+    String destinationPointName = cmd.getStep().getDestinationPoint().getName();
-    System.out.println("pathLength:" + length);
+    orderParams.put("destinationPointName", destinationPointName);
 
     //当前点位操作
     String operation = cmd.getOperation();
-    System.out.println("operation:" + operation);
+    orderParams.put("operation", operation);
-
-    //下发起点
-    String sourcePoint = cmd.getStep().getSourcePoint().getName();
-    System.out.println("sourcePoint:" + sourcePoint);
-
-    //下发终点
-    Point destinationPoint = cmd.getStep().getDestinationPoint();
-    //获取点类型
-    String pointProperty = destinationPoint.getProperty(LoopbackAdapterConstants.POINT_TYPE);
-    System.out.println("destinationPoint_tcs:point:" + pointProperty);
-    String destinationPointName = cmd.getStep().getDestinationPoint().getName();
-    System.out.println("destinationPointName:" + destinationPointName);
-
-    System.out.println("send cmd print end");
-
-    //AGV控制器执行命令------todo 待修改-》opentcs首次会下两个指令，未进入线程中不进行任务阻塞下发。
-    HybridNavigation.command(orderName, sourcePoint, destinationPointName, operation);
 
     //检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。
     if (!getProcessModel().isSingleStepModeEnabled()
@@ -338,7 +323,11 @@ public class LoopbackCommunicationAdapter
         //设置唯一订单名称
         uniqueOrderName = orderName;
         //记录订单起点---更新复位后可
-        LAST_PASSED_POINT = sourcePoint;
+        LAST_PASSED_POINT = sourcePointName;
+        if (cmd.getStep().getPath() != null) {
+          //AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞（可能会和订阅并发，如果复现需要添加延时）
+          HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation);
+        }
       }
 
       // 展示模拟车辆
@@ -574,7 +563,6 @@ public class LoopbackCommunicationAdapter
     System.out.println("start one operation");
     if (step.getPath() == null) {
       LOG.debug("-Starting operation...");
-      //无可执行路径，检查是否需要执行动作
       operationExec(command);
     } else {
       getProcessModel().getVelocityController().addWayEntry(
@@ -660,7 +648,9 @@ public class LoopbackCommunicationAdapter
       System.out.println("-emptyOperation:" + command.hasEmptyOperation());
       if (!command.hasEmptyOperation()) {
         //执行AGV动作
+        System.out.println("-Starting operation...");
         LOG.debug("-Starting operation...");
+
         operationExec(command);
       } else {
         LOG.debug("-Movement Finishing command.");
@@ -726,149 +716,68 @@ public class LoopbackCommunicationAdapter
    */
   private void operationExec(MovementCommand command) {
 
+    //关闭订阅
+    SUBSCRIBE_STATUS = false;
+
+    try {
+      //订阅概率会和动作并发，所以执行动作前需要关闭订阅并进行线程阻塞
+      Thread.sleep(SubRobotStatue.subDuration);
+    } catch (Exception e) {
+      System.out.println(e.getMessage());
+    }
+
     System.out.println("operationExec00000000000000");
 
+    //获取动作
+    String operation = command.getOperation();
 
-    if (ACTION_EXECUTION_STATUS) {
+    //下发立即动作指令
-      System.out.println("operationExec-----------");
+    if ("LIFT".equals(operation)) {
+      ActImmediately.ACTION_EXECUTE_STATE = true;
+      System.out.println("-Operation exec LoadOperation.");
+      LOG.debug("-Operation exec LoadOperation.");
 
-      //todo 读取动作执行状态
+      float height = 1.1f;
-      byte[] value = ReadValue.command();
+      byte modeOfMovement = 0x01;
-      int execStatus = value[0];
 
-      System.out.println();
+      ActImmediately.allotsOperation(height, modeOfMovement);
-      System.out.println("-execStatus:" + execStatus);
+    } else if ("DECLINE".equals(operation)) {
+      ActImmediately.ACTION_EXECUTE_STATE = true;
+      System.out.println("-Operation exec UnloadOperation.");
+      LOG.debug("-Operation exec UnloadOperation.");
 
-      if (execStatus == 1) {
+      float height = 0.1f;
-      //递归
+      byte modeOfMovement = 0x02;
-      getExecutor().schedule(
-          this::readActionState,
-          200,
-          TimeUnit.MILLISECONDS
-      );
 
-      LOG.debug("-Operation exec finished.");
+      //降叉齿指令
+      ActImmediately.allotsOperation(height, modeOfMovement);
+    } else if (operation.equals(this.getRechargeOperation())) {
+      System.out.println("-Operation exec RechargeOperation.");
+      LOG.debug("-Operation exec RechargeOperation.");
 
-      ActImmediately.reset();
+      //执行充电指令
 
-      //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),
-            500,
-            TimeUnit.MILLISECONDS
-        );
-      }
     } else {
-      System.out.println("operationExec===========");
+      System.out.println("-NOT EXECUTED OPERATION:" + operation);
-      //首次进入
+      LOG.debug("-NOT EXECUTED OPERATION:" + operation);
-      ACTION_EXECUTION_STATUS = true;
+    }
 
-      //获取动作
+    while (ActImmediately.ACTION_EXECUTE_STATE) {
-      String operation = command.getOperation();
+      try {
-      LOG.debug("-Operation exec start:" + operation);
+        //订阅概率会和动作并发，所以执行动作前需要关闭订阅并进行线程阻塞
-
+        Thread.sleep(1000);
-      //下发立即动作指令
+      } catch (Exception e) {
-      if ("LIFT".equals(operation)) {
+        System.out.println(e.getMessage());
-
-        LOG.debug("-Operation exec LoadOperation.");
-
-        float height = 1.1f;
-        byte modeOfMovement = 0x01;
-
-        allotsOperation(height, modeOfMovement);
-
-      } else if ("DECLINE".equals(operation)) {
-
-        LOG.debug("-Operation exec UnloadOperation.");
-
-
-        float height = 0.1f;
-        byte modeOfMovement = 0x02;
-
-        //降叉齿指令
-        allotsOperation(height, modeOfMovement);
-
-      } else if (operation.equals(this.getRechargeOperation())) {
-
-        LOG.debug("-Operation exec RechargeOperation.");
-
-//        allotsOperation();
-
-        //充电指令
-
-      } else {
-        LOG.debug("-Operation exec :" + operation);
-
-        //动作未定义，不执行
-        ACTION_EXECUTION_STATUS = false;
-        nextCommand();
       }
-
-      LOG.debug("-Operation exec 222222.");
-      //递归
-      getExecutor().schedule(
-          () -> operationExec(command),
-          500,
-          TimeUnit.MILLISECONDS
-      );
     }
 
-
+    //开启订阅
-    //todo-------------------
+    SUBSCRIBE_STATUS = true;
-  }
+    sub0xAF();
-
+    //完成当前命令
-  private void allotsOperation(float height, byte modeOfMovement)  {
+    finishMoveCmd(command);
-
+    //执行下一个命令
-    LOG.debug("allotsOperation111111");
+    nextCommand();
-
-    //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 execCmd = value[0];
-
-    if (execCmd == 2) {
-      getExecutor().schedule(
-          () -> readActionState(),
-          200,
-          TimeUnit.MILLISECONDS
-      );
-    }
   }
 
   /**
@@ -1195,7 +1104,7 @@ public class LoopbackCommunicationAdapter
 
   private void sub0xAF(){
     Date now = new Date();
-    sub0xafDeadline = now.getTime() + 10000;
+    sub0xafDeadline = now.getTime() + (SubRobotStatue.intervalTime * 2);
     messageProcessingPool.submit(() -> {
       SubRobotStatue.command();
     });

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

@@ -241,44 +241,47 @@ public class KCCommandDemo {
 //        }
 //      }
 
-//    {
-//
-//      //todo 读取动作执行状态
-//      byte[] value = ReadValue.command();
-//      int execStatus = value[0];
-//
-//      Thread.sleep(200);
-//
-//      while (execStatus == 2) {
-//        byte[] value1 = ReadValue.command();
-//        execStatus = value1[0];
-//      }
-//
-//
-//      ActImmediately.reset();
-//
-//      //todo 读取动作执行状态
-//      byte[] value2 = ReadValue.command();
-//      int execStatus2 = value2[0];
-//
-//      Thread.sleep(200);
-//
-//      while (execStatus2 == 2) {
-//        byte[] value1 = ReadValue.command();
-//        execStatus2 = value1[0];
-//      }
-//
-//      float height = 1.1f;
-//      byte modeOfMovement = 0x01;
-//
-//      //抬升叉齿指令
-//      ActImmediately.command(height, modeOfMovement);
-//    }
-
     {
-            ActImmediately.reset();
+
+      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();
     }
 
+//    {
+//            ActImmediately.reset();
+//    }
+
 //    {
 //
 //      //todo 读取动作执行状态

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

@@ -9,7 +9,9 @@ import org.opentcs.kc.udp.io.UDPClient;
 
 public class ActImmediately extends BaseCommand{
 
-  private static Integer actionId = 115;
+  private static Integer actionId = 1;
+
+  public static boolean ACTION_EXECUTE_STATE = false;
 
   /**
    * decs: 立即动作指令
@@ -89,4 +91,50 @@ public class ActImmediately extends BaseCommand{
     }
   }
 
+  /**
+   * 读取动作状态
+   */
+  private static void readActionState() {
+
+    try {
+      Thread.sleep(200);
+
+      //todo 读取动作执行状态
+      byte[] value = ReadValue.command();
+      int execStatus = value[0];
+
+      System.out.println();
+      System.out.println("-execStatus:" + execStatus);
+
+      while (execStatus == 2) {
+        Thread.sleep(200);
+        byte[] value1 = ReadValue.command();
+        execStatus = value1[0];
+      }
+
+      Thread.sleep(100);
+    } catch (Exception e) {
+      throw new RuntimeException("readActionState:" + e);
+    }
+  }
+
+  public static void allotsOperation(float height, byte modeOfMovement)  {
+
+    //读取动作执行状态
+    readActionState();
+    //重置
+    ActImmediately.reset();
+    //读取动作执行状态
+    readActionState();
+    //执行叉齿指令
+    ActImmediately.command(height, modeOfMovement);
+    //读取动作执行状态
+    readActionState();
+    //重置
+    ActImmediately.reset();
+    //读取重置状态
+    readActionState();
+
+    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 = 100;
+  private static int currentMaxiOrderId = 1;
 
 
   /**

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

@@ -14,7 +14,14 @@ public class SubRobotStatue
     extends
       BaseCommand {
 
+  /**
+   * 上报时间间隔
+   */
   public static final int intervalTime = 1000;
+  /**
+   * 订阅时长
+   */
+  public static int subDuration = intervalTime * 3;
 
   /**
    * decs: 下发订阅信息
@@ -25,7 +32,7 @@ public class SubRobotStatue
   public static AgvEvent issueSubscribe() {
     List<SubscribeInfo> subscribeInfoList = new ArrayList<>();
     SubscribeInfo subscribeInfo = new SubscribeInfo(
-        new byte[]{(byte) 0xaf, (byte) 0x00}, (short) intervalTime, 10000
+        new byte[]{(byte) 0xaf, (byte) 0x00}, (short) intervalTime, subDuration
     );
 //    SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xb0,(byte)0x00}, (short) 100,1000);
     subscribeInfoList.add(subscribeInfo);

opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java

@@ -124,6 +124,10 @@ public enum UDPClient {
       Package mbPackage = event.toBytes();
       AsyncFuture<Package> add = SendedList.add(mbPackage.getTransationId(), null);
 
+      if(this.conn == null || !this.conn.isActive()) {//如果连接断开，就重新连接
+        initClient();
+      }
+
       this.conn.writeAndFlush(
           new DatagramPacket(
               byteArrayToByteBuf(mbPackage.getBody()),