diff --git a/src/main/java/com/iflytop/gd/infrastructure/drivers/WebSocketCommandBusImpl.java b/src/main/java/com/iflytop/gd/infrastructure/drivers/WebSocketCommandBusImpl.java
index a2dfde8..050ea97 100644
--- a/src/main/java/com/iflytop/gd/infrastructure/drivers/WebSocketCommandBusImpl.java
+++ b/src/main/java/com/iflytop/gd/infrastructure/drivers/WebSocketCommandBusImpl.java
@@ -64,6 +64,7 @@ public class WebSocketCommandBusImpl implements CommandBus {
             throws CommandExecTimeoutException, HardwareErrorException, IOException, InterruptedException {
         try {
             if (!this.session.isOpen()) {
+                log.error("Session state={}", this.session.isOpen());
                 throw new IOException("Session is not open");
             }
             packetIndex = packetIndex + 1;
@@ -73,7 +74,7 @@ public class WebSocketCommandBusImpl implements CommandBus {
             commandPacket.setPacketIndex(packetIndex);
             this.countDownLatch = new CountDownLatch(1);
             this.session.getBasicRemote().sendText(commandPacket.toByteString());
-            boolean isTimeout = this.countDownLatch.await(timeout, unit);
+            boolean isTimeout = !this.countDownLatch.await(timeout, unit);
             // 命令返回或者超时了
             if (isTimeout) {
                 log.error("Command exec timeout, moduleId={}, commandId={}, timeoutInMilSeconds={}",
@@ -99,19 +100,14 @@ public class WebSocketCommandBusImpl implements CommandBus {
 
     @OnOpen
     public void onOpen(Session session) {
-        if (this.session != null) {
-            try {
-                this.session.close();
-            } catch (IOException e) {
-                log.error(e.getMessage(), e);
-            }
-        }
         this.session = session;
+        log.info("WebSocket connection established");
     }
 
     @OnMessage
     public void onMessage(String message) {
         byte[] bytes = ByteArray.hexStringToBytes(message);
+        log.info("New packet arrived: {}", message);
         this.lastDataPacket = new DataPacket(bytes);
         if (this.lastDataPacket.getPacketType() == DataPacket.PACKET_TYPE_ACK ||
                 this.lastDataPacket.getPacketType() == DataPacket.PACKET_TYPE_ERROR_ACK) {
diff --git a/src/main/java/com/iflytop/gd/system/models/DataPacket.java b/src/main/java/com/iflytop/gd/system/models/DataPacket.java
index 5997a59..f2fac00 100644
--- a/src/main/java/com/iflytop/gd/system/models/DataPacket.java
+++ b/src/main/java/com/iflytop/gd/system/models/DataPacket.java
@@ -34,7 +34,7 @@ public class DataPacket {
         return ByteArray.toByteString(raw);
     }
 
-    static public DataPacket createPacket(Integer moduleId, int packetType, Integer cmdId, Integer[] params) {
+    static private DataPacket createPacket(Integer moduleId, int packetType, Integer cmdId, Integer[] params) {
         int        bufferSize = DataPacket.PACKET_MIN_LEN + 4 * params.length;
         ByteBuffer buffer     = ByteBuffer.allocate(bufferSize);
         buffer.order(ByteOrder.LITTLE_ENDIAN);
@@ -61,6 +61,14 @@ public class DataPacket {
         return createPacket(moduleId, PACKET_TYPE_CMD, cmdId, params);
     }
 
+    public static DataPacket createAckPacket(Integer moduleId, Integer cmdId, Integer... params) {
+        return createPacket(moduleId, PACKET_TYPE_ACK, cmdId, params);
+    }
+
+    public static DataPacket createErrorAckPacket(Integer moduleId, Integer cmdId, Integer... params) {
+        return createPacket(moduleId, PACKET_TYPE_ERROR_ACK, cmdId, params);
+    }
+
     public int getPacketIndex() {
         return ByteArray.readU16bit(raw, INDEX_OFFSET);
     }
diff --git a/src/test/java/com/iflytop/gd/infrastructure/drivers/WebSocketCommandBusImplTest.java b/src/test/java/com/iflytop/gd/infrastructure/drivers/WebSocketCommandBusImplTest.java
new file mode 100644
index 0000000..bfd9220
--- /dev/null
+++ b/src/test/java/com/iflytop/gd/infrastructure/drivers/WebSocketCommandBusImplTest.java
@@ -0,0 +1,28 @@
+package com.iflytop.gd.infrastructure.drivers;
+
+import com.iflytop.gd.system.drivers.CommandBus;
+import com.iflytop.gd.system.exceptions.CommandExecTimeoutException;
+import com.iflytop.gd.system.exceptions.HardwareErrorException;
+import com.iflytop.gd.system.models.DataPacket;
+import jakarta.websocket.DeploymentException;
+import org.junit.jupiter.api.Test;
+
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.concurrent.TimeUnit;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+class WebSocketCommandBusImplTest {
+
+    @Test
+    void waitForCommandExec() throws DeploymentException, IOException, HardwareErrorException, CommandExecTimeoutException, InterruptedException {
+        String webSocketUrl = "ws://localhost:8888";
+        WebSocketCommandBusImpl webSocketCommandBus = new WebSocketCommandBusImpl(webSocketUrl);
+        webSocketCommandBus.connectToCommandBusWebSocketServer();
+
+        DataPacket dataPacket = DataPacket.createCommandDataPacket(ModuleId.HBotXM.index, CmdId.step_motor_easy_move_to.index, 100);
+        webSocketCommandBus.waitForCommandExec(dataPacket, 30, TimeUnit.SECONDS);
+    }
+}
\ No newline at end of file