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

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

include/navigation.hpp

@@ -1,10 +1,28 @@
-#ifndef NAVIGATION_HPP
+#ifndef __NAVIGATION_HPP__
-#define NAVIGATION_HPP
+#define __NAVIGATION_HPP__
 
-#include <string>
+#include <unitree/robot/client/client.hpp>
-#include <memory>
 #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__

src/navigation.cpp

@@ -1,21 +1,32 @@
 #include "navigation.hpp"
 #include "logger.hpp"
-#include "unitree/robot/channel/channel_factory.hpp"
-#include <iostream>
-
-const std::string SLAM_SERVICE_NAME = "slam_operate";
 
 namespace custom {
 
-Navigation::Navigation() : unitree::robot::Client(SLAM_SERVICE_NAME) {}
+Navigation::Navigation() : unitree::robot::Client(SLAM_SERVICE_NAME) {
+    subSlamInfo = ChannelSubscriberPtr(
+        new unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>(SlamInfoTopic));
+    subSlamInfo->InitChannel(
+        std::bind(&Navigation::slamInfoHandler, this, std::placeholders::_1), 1);
+    
+    subSlamKeyInfo = ChannelSubscriberPtr(
+        new unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>(SlamKeyInfoTopic));
+    subSlamKeyInfo->InitChannel(
+        std::bind(&Navigation::slamKeyInfoHandler, this, std::placeholders::_1), 1);
+}
 
 Navigation::~Navigation() {}
 
 void Navigation::Init() {
     try {
-        // Client::Init() is a pure virtual function in ClientBase and must be implemented
+        SetApiVersion(SLAM_API_VERSION);
-        // by derived classes. Since we're inheriting from Client directly, we don't need
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_POSE_NAV_PL);
-        // to explicitly call Init() as it's handled by the base class constructor.
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_PAUSE_NAV);
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_RESUME_NAV);
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_STOP_NODE);
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_START_MAPPING_PL);
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_END_MAPPING_PL);
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_START_RELOCATION_PL);
         LOG_INFO("Navigation client for service '" + SLAM_SERVICE_NAME + "' initialized.");
     } catch (const std::exception& e) {
         LOG_ERROR("Failed to initialize navigation client: " + std::string(e.what()));
@@ -55,16 +66,64 @@ bool Navigation::callSlamService(int api_id, const nlohmann::json& data) {
     }
 }
 
+void Navigation::slamInfoHandler(const void *message) {
+    std_msgs::msg::dds_::String_ currentMsg = *(std_msgs::msg::dds_::String_ *)message;
+    nlohmann::json jsonData = nlohmann::json::parse(currentMsg.data());
+
+    // Check for errors
+    if (jsonData["errorCode"] != 0) {
+        LOG_WARN("SLAM info error: " + jsonData.value("info", ""));
+        return;
+    }
+
+    // Process pose information
+    if (jsonData["type"] == "pos_info") {
+        currentPose.x = jsonData["data"]["currentPose"]["x"];
+        currentPose.y = jsonData["data"]["currentPose"]["y"];
+        currentPose.z = jsonData["data"]["currentPose"]["z"];
+        currentPose.q_x = jsonData["data"]["currentPose"]["q_x"];
+        currentPose.q_y = jsonData["data"]["currentPose"]["q_y"];
+        currentPose.q_z = jsonData["data"]["currentPose"]["q_z"];
+        currentPose.q_w = jsonData["data"]["currentPose"]["q_w"];
+        LOG_INFO("Current pose updated: x=" + std::to_string(currentPose.x) + 
+                 ", y=" + std::to_string(currentPose.y) + 
+                 ", z=" + std::to_string(currentPose.z));
+    }
+}
+
+void Navigation::slamKeyInfoHandler(const void *message) {
+    std_msgs::msg::dds_::String_ currentMsg = *(std_msgs::msg::dds_::String_ *)message;
+    nlohmann::json jsonData = nlohmann::json::parse(currentMsg.data());
+
+    // Check for errors
+    if (jsonData["errorCode"] != 0) {
+        LOG_WARN("SLAM key info error: " + jsonData.value("info", ""));
+        return;
+    }
+
+    // Process task results
+    if (jsonData["type"] == "task_result") {
+        is_arrived = jsonData["data"]["is_arrived"];
+        std::string targetNodeName = jsonData["data"]["targetNodeName"];
+        if (is_arrived) {
+            LOG_INFO("Arrived at target: " + targetNodeName);
+        } else {
+            LOG_WARN("Failed to reach target: " + targetNodeName + ". Please help!");
+        }
+    }
+}
+
+
 bool Navigation::startMapping() {
     nlohmann::json data;
     data["slam_type"] = "indoor";
-    return callSlamService(1801, data);
+    return callSlamService(ROBOT_API_ID_START_MAPPING_PL, data);
 }
 
 bool Navigation::endMapping(const std::string& address) {
     nlohmann::json data;
     data["address"] = address;
-    return callSlamService(1802, data);
+    return callSlamService(ROBOT_API_ID_END_MAPPING_PL, data);
 }
 
 bool Navigation::initializePose(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, const std::string& address) {
@@ -77,7 +136,7 @@ bool Navigation::initializePose(double x, double y, double z, double q_x, double
     data["q_z"] = q_z;
     data["q_w"] = q_w;
     data["address"] = address;
-    return callSlamService(1804, data);
+    return callSlamService(ROBOT_API_ID_START_RELOCATION_PL, data);
 }
 
 bool Navigation::poseNavigation(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, int mode, double speed) {
@@ -91,22 +150,22 @@ bool Navigation::poseNavigation(double x, double y, double z, double q_x, double
     data["targetPose"]["q_w"] = q_w;
     data["mode"] = mode;
     data["speed"] = speed;
-    return callSlamService(1102, data);
+    return callSlamService(ROBOT_API_ID_POSE_NAV_PL, data);
 }
 
 bool Navigation::pauseNavigation() {
     nlohmann::json data;
-    return callSlamService(1201, data);
+    return callSlamService(ROBOT_API_ID_PAUSE_NAV, data);
 }
 
 bool Navigation::resumeNavigation() {
     nlohmann::json data;
-    return callSlamService(1202, data);
+    return callSlamService(ROBOT_API_ID_RESUME_NAV, data);
 }
 
 bool Navigation::closeSlam() {
     nlohmann::json data;
-    return callSlamService(1901, data);
+    return callSlamService(ROBOT_API_ID_STOP_NODE, data);
 }
 
 } // namespace custom