diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp
index 8787f31..d7e86b2 100644
--- a/src/slam_wrapper.cpp
+++ b/src/slam_wrapper.cpp
@@ -3,10 +3,11 @@
 //
 
 #include "slam_wrapper.hpp"
+#include "tcp_service.hpp"
 #include <cmath>
 #include <thread>
 
-SlamWrapper::SlamWrapper(const char *networkInterface) {
+SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) {
     ChannelFactory::Instance()->Init(0, networkInterface);
 
     // pub
@@ -55,24 +56,25 @@
 
             //TODO 控制机械臂转动到预设的角度
             //朝右下方
-            // g_messageQueue.enqueue({
-            //     0xFE, 0xFE, 0x0F, 0x3C,
-            //     0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB,
-            //     0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28,
-            //     0x14, 0xFA
-            // });
+            std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl;
+            _tcp_service->send_command({
+                0xFE, 0xFE, 0x0F, 0x3C,
+                0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB,
+                0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28,
+                0x14, 0xFA
+            });
 
             //倒计时结束后恢复巡检
             std::thread countdown_thread([this] {
                 std::this_thread::sleep_for(std::chrono::seconds(10));
                 std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检";
                 //水平朝前方向
-                // g_messageQueue.enqueue({
-                //     0xFE, 0xFE, 0x0F, 0x3C,
-                //     0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
-                //     0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
-                //     0x14, 0xFA
-                // });
+                _tcp_service->send_command({
+                    0xFE, 0xFE, 0x0F, 0x3C,
+                    0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
+                    0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
+                    0x14, 0xFA
+                });
 
                 recover_navigation();
             });

diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp
index 8787f31..d7e86b2 100644
--- a/src/slam_wrapper.cpp
+++ b/src/slam_wrapper.cpp
@@ -3,10 +3,11 @@
 //
 
 #include "slam_wrapper.hpp"
+#include "tcp_service.hpp"
 #include <cmath>
 #include <thread>
 
-SlamWrapper::SlamWrapper(const char *networkInterface) {
+SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) {
     ChannelFactory::Instance()->Init(0, networkInterface);
 
     // pub
@@ -55,24 +56,25 @@
 
             //TODO 控制机械臂转动到预设的角度
             //朝右下方
-            // g_messageQueue.enqueue({
-            //     0xFE, 0xFE, 0x0F, 0x3C,
-            //     0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB,
-            //     0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28,
-            //     0x14, 0xFA
-            // });
+            std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl;
+            _tcp_service->send_command({
+                0xFE, 0xFE, 0x0F, 0x3C,
+                0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB,
+                0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28,
+                0x14, 0xFA
+            });
 
             //倒计时结束后恢复巡检
             std::thread countdown_thread([this] {
                 std::this_thread::sleep_for(std::chrono::seconds(10));
                 std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检";
                 //水平朝前方向
-                // g_messageQueue.enqueue({
-                //     0xFE, 0xFE, 0x0F, 0x3C,
-                //     0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
-                //     0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
-                //     0x14, 0xFA
-                // });
+                _tcp_service->send_command({
+                    0xFE, 0xFE, 0x0F, 0x3C,
+                    0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
+                    0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
+                    0x14, 0xFA
+                });
 
                 recover_navigation();
             });
diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp
index 80fb1f6..bc07ed4 100644
--- a/src/slam_wrapper.hpp
+++ b/src/slam_wrapper.hpp
@@ -22,6 +22,8 @@
 #define ADDNODETOPIC "rt/qt_add_node"
 #define ADDEDGETOPIC "rt/qt_add_edge"
 
+#include "tcp_service.hpp"
+
 using namespace unitree::robot;
 using namespace unitree_interfaces::msg::dds_;
 using namespace std_msgs::msg::dds_;
@@ -45,6 +47,7 @@
 private:
     int index = 0;
     const Odometry_ *currentOdom{};
+    TcpService *_tcp_service;
 
     // pub
     ChannelPublisherPtr<QtCommand_> pubQtCommand = ChannelPublisherPtr<QtCommand_>(
@@ -70,7 +73,7 @@
     std::vector<EdgeAttribute> edge_attributes;
     u_int16_t node_name = 0;
 
-    explicit SlamWrapper(const char *networkInterface);
+    explicit SlamWrapper(const char *networkInterface, TcpService *tcp_service);
 
     void qt_notice_handler(const void *message);
 

diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp
index 8787f31..d7e86b2 100644
--- a/src/slam_wrapper.cpp
+++ b/src/slam_wrapper.cpp
@@ -3,10 +3,11 @@
 //
 
 #include "slam_wrapper.hpp"
+#include "tcp_service.hpp"
 #include <cmath>
 #include <thread>
 
-SlamWrapper::SlamWrapper(const char *networkInterface) {
+SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) {
     ChannelFactory::Instance()->Init(0, networkInterface);
 
     // pub
@@ -55,24 +56,25 @@
 
             //TODO 控制机械臂转动到预设的角度
             //朝右下方
-            // g_messageQueue.enqueue({
-            //     0xFE, 0xFE, 0x0F, 0x3C,
-            //     0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB,
-            //     0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28,
-            //     0x14, 0xFA
-            // });
+            std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl;
+            _tcp_service->send_command({
+                0xFE, 0xFE, 0x0F, 0x3C,
+                0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB,
+                0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28,
+                0x14, 0xFA
+            });
 
             //倒计时结束后恢复巡检
             std::thread countdown_thread([this] {
                 std::this_thread::sleep_for(std::chrono::seconds(10));
                 std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检";
                 //水平朝前方向
-                // g_messageQueue.enqueue({
-                //     0xFE, 0xFE, 0x0F, 0x3C,
-                //     0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
-                //     0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
-                //     0x14, 0xFA
-                // });
+                _tcp_service->send_command({
+                    0xFE, 0xFE, 0x0F, 0x3C,
+                    0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
+                    0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
+                    0x14, 0xFA
+                });
 
                 recover_navigation();
             });
diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp
index 80fb1f6..bc07ed4 100644
--- a/src/slam_wrapper.hpp
+++ b/src/slam_wrapper.hpp
@@ -22,6 +22,8 @@
 #define ADDNODETOPIC "rt/qt_add_node"
 #define ADDEDGETOPIC "rt/qt_add_edge"
 
+#include "tcp_service.hpp"
+
 using namespace unitree::robot;
 using namespace unitree_interfaces::msg::dds_;
 using namespace std_msgs::msg::dds_;
@@ -45,6 +47,7 @@
 private:
     int index = 0;
     const Odometry_ *currentOdom{};
+    TcpService *_tcp_service;
 
     // pub
     ChannelPublisherPtr<QtCommand_> pubQtCommand = ChannelPublisherPtr<QtCommand_>(
@@ -70,7 +73,7 @@
     std::vector<EdgeAttribute> edge_attributes;
     u_int16_t node_name = 0;
 
-    explicit SlamWrapper(const char *networkInterface);
+    explicit SlamWrapper(const char *networkInterface, TcpService *tcp_service);
 
     void qt_notice_handler(const void *message);
 
diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp
index 584655c..2506db4 100644
--- a/src/tcp_client.cpp
+++ b/src/tcp_client.cpp
@@ -9,8 +9,7 @@
 #include <stdexcept>
 #include <thread>
 
-TcpClient::TcpClient(const char *networkInterface, TcpService *tcp_service): _slam(networkInterface),
-                                                                             _tcp_service(tcp_service) {
+TcpClient::TcpClient(const char *networkInterface, TcpService *tcp_service): _slam(networkInterface, tcp_service) {
     std::cout << "TcpClient::init success" << std::endl;
 }
 
@@ -18,30 +17,8 @@
     const std::string received_string(buffer.begin(), buffer.end());
     std::cout << "handle_data >>>> " << received_string << std::endl;
     if (received_string == "q") {
-        // std::cout << "Close all nodes" << std::endl;
-        // _slam.close_all_node();
-
-        std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒";
-        _tcp_service->send_command({
-            0xFE, 0xFE, 0x0F, 0x3C,
-            0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB,
-            0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28,
-            0x14, 0xFA
-        });
-
-        //倒计时结束后恢复巡检
-        std::thread countdown_thread([this] {
-            std::this_thread::sleep_for(std::chrono::seconds(10));
-            std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检";
-            //水平朝前方向
-            _tcp_service->send_command({
-                0xFE, 0xFE, 0x0F, 0x3C,
-                0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
-                0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
-                0x14, 0xFA
-            });
-        });
-        countdown_thread.detach();
+        std::cout << "Close all nodes" << std::endl;
+        _slam.close_all_node();
     } else if (received_string == "w") {
         std::cout << "Start mapping (default to clearing node/edge information)" << std::endl;
         _slam.delete_all_node();