commit for push

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

@@ -37,28 +37,28 @@ import org.opentcs.kc.udp.io.UDPClient;
 public class KCCommandDemo {
   public static void main(String[] args)
       throws Exception {
-        {
+//        {
-            //0xAF(查询机器人状态)
+//            //0xAF(查询机器人状态)
-            AgvEvent agvEvent = queryStatus();
+//            AgvEvent agvEvent = queryStatus();
-            printInfo(agvEvent);
+//            printInfo(agvEvent);
-            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
+//            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
-            if(rcv.isOk()){
+//            if(rcv.isOk()){
-              System.out.println();
+//              System.out.println();
-              System.out.println("received transationId : "+ "isok:"+rcv.isOk());
+//              System.out.println("received transationId : "+ "isok:"+rcv.isOk());
-              for (byte b:rcv.getValue()){
+//              for (byte b:rcv.getValue()){
-                System.out.print(byteToHex(b)+" ");
+//                System.out.print(byteToHex(b)+" ");
-              }
+//              }
-              System.out.println();
+//              System.out.println();
-              System.out.println("---------------------");
+//              System.out.println("---------------------");
-              for (byte c:rcv.getDataBytes()){
+//              for (byte c:rcv.getDataBytes()){
-                System.out.print(byteToHex(c)+" ");
+//                System.out.print(byteToHex(c)+" ");
-              }
+//              }
-                QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
+//                QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
-            }else {
+//            }else {
-                System.out.println();
+//                System.out.println();
-                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
+//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
-            }
+//            }
-        }
+//        }
 
 //        {
 //            //0xB0(查询载货状态)
@@ -224,6 +224,34 @@ public class KCCommandDemo {
             }
         }
 
+      {
+        //0xB2(立即动作指令)
+        AgvEvent agvEvent = actionNow();
+        printInfo(agvEvent);
+        RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
+        if(rcv.isOk()){
+          System.out.println("0xB2 ok");
+        }else {
+          System.out.println();
+          System.out.println("0xB2 fail");
+          System.out.println("received transationId : "+ "isok:"+rcv.isOk());
+        }
+      }
+
+    {
+      //0xB2(立即动作指令)
+      AgvEvent agvEvent = navigationControl();
+      printInfo(agvEvent);
+      RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
+      if(rcv.isOk()){
+        System.out.println("0xAE ok");
+      }else {
+        System.out.println();
+        System.out.println("0xAE fail");
+        System.out.println("received transationId : "+ "isok:"+rcv.isOk());
+      }
+    }
+
   }
 
 
@@ -312,6 +340,60 @@ public class KCCommandDemo {
     return agvEvent;
   }
 
+  /**
+   * decs: 立即动作指令
+   * 指令：0xB2
+   * author: caixiang
+   * date: 2025/1/17 16:25
+   */
+  public static AgvEvent actionNow() {
+    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY);
+    //TODO 构建
+    Integer orderId = 1;
+
+    //构建Action指令
+    Action actionLiftUp = new Action(
+        ActionSet.stop0x00, //机器人移动到正常点 停止，2个字节
+        (byte) 0x02,   //只能执行当前动作
+        1,     //actionID
+        ActionSet.fork0x12_paramsize,
+        ActionSet.fork0x12(
+          1.1f,   //升降高度
+          (byte) 0x01,  //1上升  2下降
+            (byte)0x01   //01 开始 任务
+        )
+    );
+    agvEvent.setBody(actionLiftUp.toBytes());
+//
+//    //重置 空指令
+//    Action actionReset = new Action(
+//        ActionSet.stop0x00, //机器人移动到正常点 停止，2个字节
+//        (byte) 0x02,   //只能执行当前动作
+//        2,     //actionID
+//        ActionSet.cancle0x03_paramsize,
+//        ActionSet.cancel0x12(
+//            1,   //订单id
+//            (byte)0x01   //1 立即停止， 2 正常移动到点再停止
+//        )
+//    );
+//    agvEvent.setBody(actionReset.toBytes());
+//
+//    //构建Action指令
+//    Action actionLiftDown = new Action(
+//        ActionSet.stop0x00, //机器人移动到正常点 停止，2个字节
+//        (byte) 0x02,   //只能执行当前动作
+//        1,     //actionID
+//        ActionSet.fork0x12_paramsize,
+//        ActionSet.fork0x12(
+//            1.1f,   //升降高度
+//            (byte) 0x02,  //1上升  2下降
+//            (byte)0x01   //01 开始 任务
+//        )
+//    );
+//    agvEvent.setBody(actionLiftDown.toBytes());
+    return agvEvent;
+  }
+
   /**
    * decs: 导航控制
    * 指令：0xAE

opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/ActionSet.java

@@ -11,6 +11,9 @@ public class ActionSet {
   }
 
   public static byte stop0x01 = 0x01;
+  public static byte stop0x00 = 0x00;
+
+
   public static byte recover0x02 = 0x02;
   public static byte cancelTask0x03 = 0x03;
   public static byte forkliftElevation0x12 = 0x12;
@@ -18,6 +21,8 @@ public class ActionSet {
 
 
   public static Integer stop0x01_paramsize = 8;
+  public static Integer fork0x12_paramsize = 8;
+  public static Integer cancle0x03_paramsize = 8;
   public static Integer recover0x02_paramsize = 8;
   public static Integer cancelTask0x03_paramsize = 8;
   public static Integer forkliftElevation0x12_paramsize = 8;
@@ -31,6 +36,32 @@ public class ActionSet {
     return byteBuf.array();
   }
 
+  /**
+   * high : 升降高度
+   * type： 1 上升  ； 2 下降
+   * command : 0：无任务
+   *          1：开始任务
+   *          2：暂停任务
+   *          3：继续任务
+   *          4：放弃任务
+   * */
+  public static byte[] fork0x12(Float high, byte type,byte command) {
+    ByteBuf byteBuf = Unpooled.buffer(8);
+    byteBuf.writeBytes(ByteUtils.floatToBytes(high));
+    byteBuf.writeByte((byte)0x01);
+    byteBuf.writeByte(type);
+    byteBuf.writeByte(command);
+    byteBuf.writeByte((byte)0x00);
+    return byteBuf.array();
+  }
+
+  public static byte[] cancel0x12(Integer orderId, byte type) {
+    ByteBuf byteBuf = Unpooled.buffer(8);
+    byteBuf.writeBytes(ByteUtils.intToBytes(orderId, 1));
+    byteBuf.writeByte(type);
+    return byteBuf.array();
+  }
+
 
   public static byte[] recover0x02(Integer orderId, Integer taskKey) {
     ByteBuf byteBuf = Unpooled.buffer(8);

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

@@ -31,7 +31,7 @@ import org.slf4j.LoggerFactory;
 public enum UDPClient {
   //如果要配置多个链接， local1 local2 .... 这样排下去好了
 
-  localAGV("50", "127.0.0.1", 17804, 55678),
+  localAGV("50", "192.168.124.124", 17804, 55678),
   ;
 
   // 服务端用户名+密码+地址+端口

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

@@ -12,7 +12,7 @@ public class main {
 
 
     //size 148 ,contentSize = 148-28 = 120
-    String errSrc = "d4 97 44 9c cb cf 0b 4c 95 51 d8 61 70 f1 e7 94 01 01 00 00 10 af 00 00 00 68 00 00 00 00 00 00 53 3a 48 bd 32 fb 7b bc ca 13 03 ba 01 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 01 00 00 00 00 00 00 05 00 00 00 01 00 00 00 01 01 00 00 02 00 00 00 02 00 00 00 01 00 00 00 0c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00\n";
+    String errSrc = "d4 97 44 9c cb cf 0b 4c 95 51 d8 61 70 f1 e7 94 01 01 00 00 10 af 00 00 00 68 00 00 00 00 00 00 53 3a 48 bd 32 fb 7b bc ca 13 03 ba 01 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 01 00 00 00 00 00 00 05 00 00 00 01 00 00 00 01 01 00 00 02 00 00 00 02 00 00 00 01 00 00 00 0c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00";
     String okSrc = "d4 97 44 9c cb cf 0b 4c 95 51 d8 61 70 f1 e7 94 01 01 03 00 10 af 00 00 64 00 00 00 01 00 00 00 73 1c 30 3d 66 ba 03 3d d9 51 59 bb 00 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 0a 01 02 00 00 00 00 00 00 00 00 00 73 1c 30 3d 66 ba 03 3d d9 51 59 bb 00 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 ";
     String[] s = errSrc.split(" ");
     System.out.println("size: " + s.length);