diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp
index 8be0ebf..3700351 100644
--- a/src/slam_wrapper.cpp
+++ b/src/slam_wrapper.cpp
@@ -19,34 +19,49 @@
 }
 
 void SlamWrapper::qt_notice_handler(const void *message) {
-    int index_, begin_, end_, feedback_, arrive_;
     const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message;
-    std::string str_, notice_;
+    std::string str_;
 
-    begin_ = seq->data().find("index:", 0); // Instruction unique identifier
-    end_ = seq->data().find(";", begin_);
+    int begin_ = seq->data().find("index:", 0); // Instruction unique identifier
+    int end_ = seq->data().find(";", begin_);
     str_ = seq->data().substr(begin_ + 6, end_ - begin_ - 6);
-    index_ = atoi(str_.c_str());
+    char *endptr = nullptr;
+    if (*endptr != '\0') {
+        std::cerr << "Error converting index to integer" << std::endl;
+        return;
+    }
+    const int index = static_cast<int>(strtol(str_.c_str(), &endptr, 10));
 
     begin_ = seq->data().find("notice:", 0); // Prompt message
     end_ = seq->data().find(";", begin_);
-    notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7);
+    std::string notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7);
 
-    if (index_ <= 10000) {
+    if (index <= 10000) {
         // Command execution feedback
         begin_ = seq->data().find("feedback:", 0);
         end_ = seq->data().find(";", begin_);
         str_ = seq->data().substr(begin_ + 9, end_ - begin_ - 9);
-        feedback_ = atoi(str_.c_str());
-        std::cout << "index:" << index_ << " feedback:" << feedback_ << " notice:" << notice_ << std::endl;
-    } else if (index_ == 10001) {
+        const long feedback_ = strtol(str_.c_str(), &endptr, 10);
+        if (*endptr != '\0') {
+            std::cerr << "Error converting feedback to integer" << std::endl;
+            return;
+        }
+        std::cout << "index:" << index << " feedback:" << feedback_ << " notice:" << notice_ << std::endl;
+    } else if (index == 10001) {
         // Navigation feedback
         begin_ = seq->data().find("arrive:", 0);
         end_ = seq->data().find(";", begin_);
         str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7);
-        // 巡检到达的位置
-        arrive_ = atoi(str_.c_str());
-        std::cout << "index:" << index_ << " arrive:" << arrive_ << " notice:" << notice_ << std::endl;
+        // TODO 巡检到达的位置。判断到达的是第一个点，然后发送暂停指令，控制机械臂转动到预设的角度，几秒后恢复巡检
+        const long arrive = strtol(str_.c_str(), &endptr, 10);
+        if (*endptr != '\0') {
+            std::cerr << "Error converting arrive to integer" << std::endl;
+            return;
+        }
+        std::cout << "index:" << index << " arrive:" << arrive << " notice:" << notice_ << std::endl;
+        if (arrive == 1) {
+            pause_navigation();
+        }
     }
 }
 
@@ -65,7 +80,7 @@
 }
 
 void SlamWrapper::end_mapping() {
-    index++;
+    index++; //
     unitree_interfaces::msg::dds_::QtCommand_ send_msg;
     send_msg.command_() = 4; // 4 is the end mapping command
     send_msg.attribute_() = 0;
@@ -118,12 +133,17 @@
 
 void SlamWrapper::add_node_and_edge() {
     node_attribute nodeTemp{};
-    float siny_cosp, cosy_cosp;
 
-    siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() * currentOdom->pose().pose().orientation().z() +
-                     currentOdom->pose().pose().orientation().x() * currentOdom->pose().pose().orientation().y());
-    cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() * currentOdom->pose().pose().orientation().y() +
-                         currentOdom->pose().pose().orientation().z() * currentOdom->pose().pose().orientation().z());
+    const float siny_cosp = 2 * (currentOdom
+                                 ->pose().pose().orientation().w() * currentOdom
+                                 ->pose().pose().orientation().z() + currentOdom
+                                 ->pose().pose().orientation().x() * currentOdom
+                                 ->pose().pose().orientation().y());
+    const float cosy_cosp = 1 - 2 * (currentOdom
+                                     ->pose().pose().orientation().y() * currentOdom
+                                     ->pose().pose().orientation().y() + currentOdom
+                                     ->pose().pose().orientation().z() * currentOdom
+                                     ->pose().pose().orientation().z());
     node_name++;
 
     nodeTemp.nodeX = currentOdom->pose().pose().position().x();
@@ -139,7 +159,7 @@
     }
 }
 
-void SlamWrapper::add_edge(u_int16_t edge_name, u_int16_t start_node, u_int16_t end_node) {
+void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) {
     edge_attribute edgeTemp{};
 
     edgeTemp.edgeName = edge_name;