feat(导航): 实现SLAM信息处理和导航状态管理

添加SLAM信息订阅处理功能，包括位姿更新和任务结果处理
引入PoseData结构体存储当前位姿信息
使用常量定义替换硬编码的API ID
增加导航状态管理功能(到达检测)

include/navigation.hpp

@@ -1,10 +1,28 @@
-#ifndef NAVIGATION_HPP
-#define NAVIGATION_HPP
+#ifndef __NAVIGATION_HPP__
+#define __NAVIGATION_HPP__
 
-#include <string>
-#include <memory>
+#include <unitree/robot/client/client.hpp>
 #include <nlohmann/json.hpp>
-#include "unitree/robot/client/client.hpp"
+#include <string>
+#include <iostream>
+
+#include "std_msgs/msg/dds_/String_.hpp"
+#include "unitree/robot/channel/channel_subscriber.hpp"
+
+
+const std::string SLAM_SERVICE_NAME = "slam_operate";
+const std::string SLAM_API_VERSION = "1.0.0";
+const int32_t ROBOT_API_ID_STOP_NODE = 1901;
+const int32_t ROBOT_API_ID_START_MAPPING_PL = 1801;
+const int32_t ROBOT_API_ID_END_MAPPING_PL = 1802;
+const int32_t ROBOT_API_ID_START_RELOCATION_PL = 1804;
+const int32_t ROBOT_API_ID_POSE_NAV_PL = 1102;
+const int32_t ROBOT_API_ID_PAUSE_NAV = 1201;
+const int32_t ROBOT_API_ID_RESUME_NAV = 1202;
+
+// DDS topic names
+#define SlamInfoTopic "rt/slam_info"
+#define SlamKeyInfoTopic "rt/slam_key_info"
 
 namespace custom {
 
@@ -13,7 +31,7 @@ public:
     Navigation();
     ~Navigation();
 
-    void Init();
+    void Init() override;
 
     bool startMapping();
     bool endMapping(const std::string& address);
@@ -23,10 +41,33 @@ public:
     bool resumeNavigation();
     bool closeSlam();
 
+    PoseData getCurrentPose() const { return currentPose; }
+
+    bool hasArrived() const { return is_arrived; }
+
 private:
     bool callSlamService(int api_id, const nlohmann::json& data);
+    void slamInfoHandler(const void *message);
+    void slamKeyInfoHandler(const void *message);
+
+    ChannelSubscriberPtr subSlamInfo;
+    ChannelSubscriberPtr subSlamKeyInfo;
+
+    // Structure to hold pose data
+    struct PoseData {
+        double x = 0.0;
+        double y = 0.0;
+        double z = 0.0;
+        double q_x = 0.0;
+        double q_y = 0.0;
+        double q_z = 0.0;
+        double q_w = 0.0;
+    };
+
+    PoseData currentPose;
+    bool is_arrived = false;
 };
 
 } // namespace custom
 
-#endif // NAVIGATION_HPP
+#endif // __NAVIGATION_HPP__