Add navigation functionality to CustomRobot and Controller classes. Introduced Navigation class for handling mapping and pose navigation commands. Updated CustomRobot to process navigation commands and manage Navigation instance. Enhanced Controller with motion switching capabilities, improving overall robot control and navigation features.

src/navigation.cpp

@@ -0,0 +1,112 @@
+#include "navigation.hpp"
+#include "logger.hpp"
+#include "unitree/robot/channel/channel_factory.hpp"
+#include <iostream>
+
+const std::string SLAM_SERVICE_NAME = "slam_operate";
+
+Navigation::Navigation() : client_(nullptr) {}
+
+Navigation::~Navigation() {}
+
+void Navigation::Init() {
+    try {
+        client_ = std::make_unique<unitree::robot::Client>(SLAM_SERVICE_NAME);
+        client_->Init();
+        LOG_INFO("Navigation client for service '{}' initialized.", SLAM_SERVICE_NAME);
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to initialize navigation client: {}", e.what());
+    }
+}
+
+bool Navigation::callSlamService(int api_id, const nlohmann::json& data) {
+    if (!client_) {
+        LOG_ERROR("Navigation client not initialized.");
+        return false;
+    }
+
+    nlohmann::json request_json;
+    request_json["data"] = data;
+    std::string request_str = request_json.dump();
+    std::string response_str;
+
+    LOG_INFO("Calling slam service api_id: {}, request: {}", api_id, request_str);
+    
+    int ret = client_->Call(api_id, request_str, response_str);
+
+    if (ret != 0) {
+        LOG_ERROR("Slam service request failed with SDK error code: {}", ret);
+        return false;
+    }
+    
+    LOG_INFO("Slam service response: {}", response_str);
+
+    try {
+        nlohmann::json response_json = nlohmann::json::parse(response_str);
+        if (response_json.contains("succeed") && response_json["succeed"].get<bool>()) {
+            LOG_INFO("Slam service call for api_id {} succeeded. Info: {}", api_id, response_json.value("info", ""));
+            return true;
+        } else {
+            LOG_ERROR("Slam service call for api_id {} failed. Error code: {}, Info: {}", 
+                      api_id, response_json.value("errorCode", -1), response_json.value("info", ""));
+            return false;
+        }
+    } catch (const nlohmann::json::parse_error& e) {
+        LOG_ERROR("Failed to parse slam service response: {}", e.what());
+        return false;
+    }
+}
+
+bool Navigation::startMapping() {
+    nlohmann::json data;
+    data["slam_type"] = "indoor";
+    return callSlamService(1801, data);
+}
+
+bool Navigation::endMapping(const std::string& address) {
+    nlohmann::json data;
+    data["address"] = address;
+    return callSlamService(1802, 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) {
+    nlohmann::json data;
+    data["x"] = x;
+    data["y"] = y;
+    data["z"] = z;
+    data["q_x"] = q_x;
+    data["q_y"] = q_y;
+    data["q_z"] = q_z;
+    data["q_w"] = q_w;
+    data["address"] = address;
+    return callSlamService(1804, 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) {
+    nlohmann::json data;
+    data["targetPose"]["x"] = x;
+    data["targetPose"]["y"] = y;
+    data["targetPose"]["z"] = z;
+    data["targetPose"]["q_x"] = q_x;
+    data["targetPose"]["q_y"] = q_y;
+    data["targetPose"]["q_z"] = q_z;
+    data["targetPose"]["q_w"] = q_w;
+    data["mode"] = mode;
+    data["speed"] = speed;
+    return callSlamService(1102, data);
+}
+
+bool Navigation::pauseNavigation() {
+    nlohmann::json data;
+    return callSlamService(1201, data);
+}
+
+bool Navigation::resumeNavigation() {
+    nlohmann::json data;
+    return callSlamService(1202, data);
+}
+
+bool Navigation::closeSlam() {
+    nlohmann::json data;
+    return callSlamService(1901, data);
+}