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.

2025-09-20 16:05:10 +08:00
parent 2de8f53ae2
commit 38db96e433
7 changed files with 462 additions and 195 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -34,6 +34,7 @@ set(SOURCES
    src/config.cpp
    src/logger.cpp
    src/mqtt.cpp
+    src/navigation.cpp
 )

 # Create executable with simple name
--- a/include/controller.hpp
+++ b/include/controller.hpp
@@ -6,10 +6,12 @@

 #include <unitree/robot/go2/sport/sport_client.hpp>
 #include <unitree/robot/go2/obstacles_avoid/obstacles_avoid_client.hpp>
+#include <unitree/robot/go2/motion_switcher/motion_switcher_client.hpp>
 #include <unitree/robot/go2/video/video_client.hpp>
 #include <unitree/robot/go2/vui/vui_client.hpp>
 #include <unitree/robot/channel/channel_subscriber.hpp>
 #include <unitree/idl/go2/SportModeState_.hpp>
+#include "logger.hpp"

 namespace custom {

@@ -45,10 +47,24 @@ public:
    bool MoveToAbsolutePosition(float x, float y, float yaw);
    bool MoveToIncrementPosition(float x, float y, float yaw);
    
+    // MotionSwitcher
+    bool CheckMode(std::string& form, std::string& name);
+    bool SelectMode(const std::string& nameOrAlias);
+    bool ReleaseMode();

 private:
+    template<typename Func>
+    bool ExecuteSportCmd(Func&& func);
+    
+    template<typename Func>
+    bool ExecuteObstacleCmd(Func&& func);
+
+    template<typename Func>
+    bool ExecuteMotionSwitchCmd(Func&& func);
+    
    std::unique_ptr<unitree::robot::go2::SportClient> sc_;
    std::unique_ptr<unitree::robot::go2::ObstaclesAvoidClient> oac_;
+    std::unique_ptr<unitree::robot::go2::MotionSwitcherClient> msc_;
    
    std::atomic<bool> running_;
    std::atomic<bool> initialized_;
--- a/include/custom_robot.hpp
+++ b/include/custom_robot.hpp
@@ -4,6 +4,7 @@
 #include "config.hpp"
 #include "logger.hpp"
 #include "mqtt.hpp"
+#include "navigation.hpp"

 #include <memory>
 #include <atomic>
@@ -23,6 +24,7 @@ public:
    bool start();
    
    Controller* getController() const { return controller_.get(); }
+    Navigation* getNavigation() const { return navigation_.get(); }
    
    bool GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList);
    bool SwitchService(const std::string& serviceName, bool enable);
@@ -37,11 +39,13 @@ public:
    bool processOacCmd(const std::string& cmd, const nlohmann::json& message);
    bool processSportCmd(const std::string& cmd, const nlohmann::json& message);
    bool processRscCmd(const std::string& cmd, const nlohmann::json& message);
+    bool processNavCmd(const std::string& cmd, const nlohmann::json& message);
    void printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus = -1);

 private:
    std::string generateRandomClientId() const;
    std::unique_ptr<Controller> controller_;
+    std::unique_ptr<Navigation> navigation_;
    std::unique_ptr<unitree::robot::go2::RobotStateClient> rsc_;
    std::unique_ptr<MqttClient> mqttClient_;
    
--- a/include/navigation.hpp
+++ b/include/navigation.hpp
@@ -0,0 +1,29 @@
+#ifndef NAVIGATION_HPP
+#define NAVIGATION_HPP
+
+#include <string>
+#include <memory>
+#include <nlohmann/json.hpp>
+#include "unitree/robot/client/client.hpp"
+
+class Navigation {
+public:
+    Navigation();
+    ~Navigation();
+
+    void Init();
+
+    bool startMapping();
+    bool endMapping(const std::string& address);
+    bool initializePose(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, const std::string& address);
+    bool poseNavigation(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, int mode, double speed);
+    bool pauseNavigation();
+    bool resumeNavigation();
+    bool closeSlam();
+
+private:
+    std::unique_ptr<unitree::robot::Client> client_;
+    bool callSlamService(int api_id, const nlohmann::json& data);
+};
+
+#endif // NAVIGATION_HPP
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -2,6 +2,9 @@
 #include "config.hpp"
 #include "logger.hpp"
 #include <unitree/common/thread/thread.hpp>
+#include <iostream>
+#include <unistd.h>
+#include <string.h>

 namespace custom {

@@ -17,6 +20,7 @@ Controller::~Controller() {
    LOG_INFO("Shutting down robot controller");
    sc_.reset();
    oac_.reset();
+    msc_.reset();
    initialized_ = false;
 }

@@ -30,6 +34,9 @@ bool Controller::initialize() {
        oac_->SetTimeout(3.0f);
        oac_->Init();
        
+        msc_ = std::make_unique<unitree::robot::go2::MotionSwitcherClient>();
+        msc_->SetTimeout(10.0f);
+        msc_->Init();
        
        initialized_ = true;
        return true;
@@ -67,236 +74,188 @@ bool Controller::stop() {
    return true;
 }

+template<typename Func>
+bool Controller::ExecuteSportCmd(Func&& func) {
+    if (!initialized_) {
+        LOG_ERROR("Controller not initialized");
+        return false;
+    }
    
+    if (!sc_) {
+        LOG_ERROR("SportClient not available");
+        return false;
+    }
+    
+    try {
+        int32_t result = func(sc_.get());
+        return result == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Command failed with exception: " + std::string(e.what()));
+        return false;
+    } catch (...) {
+        LOG_ERROR("Command failed with unknown exception");
+        return false;
+    }
+}
+
+template<typename Func>
+bool Controller::ExecuteObstacleCmd(Func&& func) {
+    if (!initialized_) {
+        LOG_ERROR("Controller not initialized");
+        return false;
+    }
+    
+    if (!oac_) {
+        LOG_ERROR("ObstaclesAvoidClient not available");
+        return false;
+    }
+    
+    try {
+        int32_t result = func(oac_.get());
+        return result == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Command failed with exception: " + std::string(e.what()));
+        return false;
+    } catch (...) {
+        LOG_ERROR("Command failed with unknown exception");
+        return false;
+    }
+}
+
+template<typename Func>
+bool Controller::ExecuteMotionSwitchCmd(Func&& func) {
+    if (!initialized_) {
+        LOG_ERROR("Controller not initialized");
+        return false;
+    }
+    
+    if (!msc_) {
+        LOG_ERROR("MotionSwitcherClient not available");
+        return false;
+    }
+    
+    try {
+        int32_t result = func(msc_.get());
+        return result == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Command failed with exception: " + std::string(e.what()));
+        return false;
+    } catch (...) {
+        LOG_ERROR("Command failed with unknown exception");
+        return false;
+    }
+}


 bool Controller::StandUp() {
-    LOG_INFO("Standing up");
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->StandUp();
-        LOG_INFO("StandUp API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Stand up failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->StandUp();
+    });
 }

 bool Controller::StandDown() {
-    LOG_INFO("Standing down");
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->StandDown();
-        LOG_INFO("StandDown API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Stand down failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->StandDown();
+    });
 }

 bool Controller::Sit() {
-    LOG_INFO("Sitting");
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->Sit();
-        LOG_INFO("Sit API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Sit failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->Sit();
+    });
 }

 bool Controller::Damp() {
-    LOG_INFO("Damping");
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->Damp();
-        LOG_INFO("Damp API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Damp failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->Damp();
+    });
 }

 bool Controller::RecoveryStand() {
-    LOG_INFO("Executing recovery stand");
-    
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->RecoveryStand();
-        LOG_INFO("RecoveryStand API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Recovery stand failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->RecoveryStand();
+    });
 }

 bool Controller::StopMove() {
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->StopMove();
-        LOG_INFO("StopMove API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Stop move failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->StopMove();
+    });
 }

 bool Controller::BalanceStand() {
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->BalanceStand();
-        LOG_INFO("BalanceStand API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Balance stand failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->BalanceStand();
+    });
 }

 bool Controller::Dance1() {
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->Dance1();
-        LOG_INFO("Dance1 API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Dance1 failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->Dance1();
+    });
 }

 bool Controller::Dance2() {
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->Dance2();
-        LOG_INFO("Dance2 API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Dance2 failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->Dance2();
+    });
 }

 bool Controller::Hello() {
-    try {
-        if (!sc_) {
-            LOG_ERROR("SportClient not initialized");
-            return false;
-        }
-        
-        int32_t result = sc_->Hello();
-        LOG_INFO("Hello API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Hello failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteSportCmd([](auto* sc) {
+        return sc->Hello();
+    });
 }




 bool Controller::SwitchSet(bool enable) {
-    try {
-        if (!oac_) {
-            LOG_ERROR("ObstaclesAvoidClient not initialized");
-            return false;
-        }
-        
-        int32_t result = oac_->SwitchSet(enable);
-        LOG_INFO("SwitchSet API returned: " + std::to_string(result));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Switch set failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteObstacleCmd([enable](auto* oac) {
+        return oac->SwitchSet(enable);
+    });
 }

 bool Controller::SwitchGet(bool& enable) {
-    try {
-        if (!oac_) {
-            LOG_ERROR("ObstaclesAvoidClient not initialized");
-            return false;
-        }
-        
-        int32_t result = oac_->SwitchGet(enable);
-        LOG_INFO("SwitchGet API returned: " + std::to_string(result) + 
-                ", enable: " + std::string(enable ? "true" : "false"));
-        return result == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Switch get failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteObstacleCmd([&enable](auto* oac) {
+        return oac->SwitchGet(enable);
+    });
 }

 bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) {
-    try {
-        return oac_ && oac_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Use remote command from api failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteObstacleCmd([isRemoteCommandsFromApi](auto* oac) {
+        return oac->UseRemoteCommandFromApi(isRemoteCommandsFromApi);
+    });
 }

 bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) {
-    try {
-        return oac_ && oac_->MoveToAbsolutePosition(x, y, yaw) == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Move to absolute position failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteObstacleCmd([x, y, yaw](auto* oac) {
+        return oac->MoveToAbsolutePosition(x, y, yaw);
+    });
 }

 bool Controller::MoveToIncrementPosition(float x, float y, float yaw) {
-    try {
-        return oac_ && oac_->MoveToIncrementPosition(x, y, yaw) == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Move to increment position failed: " + std::string(e.what()));
-        return false;
-    }
+    return ExecuteObstacleCmd([x, y, yaw](auto* oac) {
+        return oac->MoveToIncrementPosition(x, y, yaw);
+    });
+}
+
+bool Controller::CheckMode(std::string& form, std::string& name) {
+    return ExecuteMotionSwitchCmd([&](auto* msc) {
+        return msc->CheckMode(form, name);
+    });
+}
+
+bool Controller::SelectMode(const std::string& nameOrAlias) {
+    return ExecuteMotionSwitchCmd([&](auto* msc) {
+        return msc->SelectMode(nameOrAlias);
+    });
+}
+
+bool Controller::ReleaseMode() {
+    return ExecuteMotionSwitchCmd([&](auto* msc) {
+        return msc->ReleaseMode();
+    });
 }

 } // namespace custom
--- a/src/custom_robot.cpp
+++ b/src/custom_robot.cpp
@@ -12,7 +12,7 @@
 namespace custom {

 CustomRobot::CustomRobot()
-    : initialized_(false), running_(false) {
+    : initialized_(false), running_(false), controller_(nullptr), navigation_(nullptr), rsc_(nullptr), mqttClient_(nullptr) {
    config_.loadDefaults();
    
    try {
@@ -42,6 +42,10 @@ CustomRobot::~CustomRobot() {
        rsc_.reset();
    }

+    if (navigation_) {
+        navigation_.reset();
+    }
+    
    try {
        unitree::robot::ChannelFactory::Instance()->Release();
        LOG_INFO("ChannelFactory released");
@@ -57,6 +61,9 @@ bool CustomRobot::initialize() {
        controller_ = std::make_unique<Controller>();
        controller_->initialize();

+        navigation_ = std::make_unique<Navigation>();
+        navigation_->Init();
+
        rsc_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
        rsc_->SetTimeout(3.0f);
        rsc_->Init();
@@ -244,6 +251,8 @@ void CustomRobot::processCmd(const nlohmann::json& message) {
            success = processSportCmd(cmd, message);
        } else if (type == "rsc") {
            success = processRscCmd(cmd, message);
+        } else if (type == "nav") {
+            success = processNavCmd(cmd, message);
        } else {
            LOG_ERROR("Unknown command type: " + type);
            return;
@@ -330,6 +339,71 @@ bool CustomRobot::processOacCmd(const std::string& cmd, const nlohmann::json& me
    }
 }

+bool CustomRobot::processNavCmd(const std::string& cmd, const nlohmann::json& message) {
+    if (!navigation_) {
+        LOG_ERROR("Navigation not initialized");
+        return false;
+    }
+
+    try {
+        if (cmd == "start_mapping") {
+            return navigation_->startMapping();
+        } else if (cmd == "end_mapping") {
+            if (!message.contains("param") || !message["param"].contains("address")) {
+                LOG_ERROR("end_mapping cmd missing 'address' parameter");
+                return false;
+            }
+            std::string address = message["param"]["address"];
+            return navigation_->endMapping(address);
+        } else if (cmd == "initialize_pose") {
+            if (!message.contains("param")) {
+                LOG_ERROR("initialize_pose cmd missing 'param'");
+                return false;
+            }
+            auto& param = message["param"];
+            const std::vector<std::string> keys = {"x", "y", "z", "q_x", "q_y", "q_z", "q_w", "address"};
+            for (const auto& key : keys) {
+                if (!param.contains(key)) {
+                    LOG_ERROR("initialize_pose missing parameter: " + key);
+                    return false;
+                }
+            }
+            return navigation_->initializePose(param["x"], param["y"], param["z"], param["q_x"], param["q_y"], param["q_z"], param["q_w"], param["address"]);
+        } else if (cmd == "pose_navigation") {
+            if (!message.contains("param")) {
+                LOG_ERROR("pose_navigation cmd missing 'param'");
+                return false;
+            }
+            auto& param = message["param"];
+            if (!param.contains("targetPose") || !param.contains("mode") || !param.contains("speed")) {
+                LOG_ERROR("pose_navigation missing 'targetPose', 'mode', or 'speed'");
+                return false;
+            }
+            auto& targetPose = param["targetPose"];
+            const std::vector<std::string> pose_keys = {"x", "y", "z", "q_x", "q_y", "q_z", "q_w"};
+             for (const auto& key : pose_keys) {
+                if (!targetPose.contains(key)) {
+                    LOG_ERROR("pose_navigation.targetPose missing parameter: " + key);
+                    return false;
+                }
+            }
+            return navigation_->poseNavigation(targetPose["x"], targetPose["y"], targetPose["z"], targetPose["q_x"], targetPose["q_y"], targetPose["q_z"], targetPose["q_w"], param["mode"], param["speed"]);
+        } else if (cmd == "pause_navigation") {
+            return navigation_->pauseNavigation();
+        } else if (cmd == "resume_navigation") {
+            return navigation_->resumeNavigation();
+        } else if (cmd == "close_slam") {
+            return navigation_->closeSlam();
+        } else {
+            LOG_ERROR("Unknown Nav command: " + cmd);
+            return false;
+        }
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error executing Nav command " + cmd + ": " + std::string(e.what()));
+        return false;
+    }
+}
+
 bool CustomRobot::processRscCmd(const std::string& cmd, const nlohmann::json& message) {
    try {
        if (cmd == "list") {
@@ -417,27 +491,99 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
    }
    
    try {
-        if (cmd == "StandUp") {
+        if (cmd == "Damp") {
+            return controller_->Damp();
+        } else if (cmd == "BalanceStand") {
+            return controller_->BalanceStand();
+        } else if (cmd == "StopMove") {
+            return controller_->StopMove();
+        } else if (cmd == "StandUp") {
            return controller_->StandUp();
        } else if (cmd == "StandDown") {
            return controller_->StandDown();
-        } else if (cmd == "Sit") {
-            return controller_->Sit();
-        } else if (cmd == "Damp") {
-            return controller_->Damp();
        } else if (cmd == "RecoveryStand") {
            return controller_->RecoveryStand();
-        } else if (cmd == "StopMove") {
-            return controller_->StopMove();
-        } else if (cmd == "BalanceStand") {
-            return controller_->BalanceStand();
+        } else if (cmd == "Euler") {
+            return controller_->Euler(message["param"]["roll"], message["param"]["pitch"], message["param"]["yaw"]);
+        } else if (cmd == "Move") {
+            return controller_->Move(message["param"]["vx"], message["param"]["vy"], message["param"]["vyaw"]);
+        } else if (cmd == "Sit") {
+            return controller_->Sit();
+        } else if (cmd == "RiseSit") {
+            return controller_->RiseSit();
+        } else if (cmd == "SpeedLevel") {
+            return controller_->SpeedLevel(message["param"]["level"]);
+        } else if (cmd == "Hello") {
+            return controller_->Hello();
+        } else if (cmd == "Stretch") {
+            return controller_->Stretch();
+        } else if (cmd == "SwitchJoystick") {
+            return controller_->SwitchJoystick(message["param"]["flag"]);
+        } else if (cmd == "Content") {
+            return controller_->Content();
+        } else if (cmd == "Heart") {
+            return controller_->Heart();
+        } else if (cmd == "Pose") {
+            return controller_->Pose(message["param"]["flag"]);
+        } else if (cmd == "Scrape") {
+            return controller_->Scrape();
+        } else if (cmd == "FrontFlip") {
+            return controller_->FrontFlip();
+        } else if (cmd == "FrontJump") {
+            return controller_->FrontJump();
+        } else if (cmd == "FrontPounce") {
+            return controller_->FrontPounce();
        } else if (cmd == "Dance1") {
            return controller_->Dance1();
        } else if (cmd == "Dance2") {
            return controller_->Dance2();
-        } else if (cmd == "Hello") {
-            return controller_->Hello();
-        } else {
+        } else if (cmd == "LeftFlip") {
+            return controller_->LeftFlip();
+        } else if (cmd == "BackFlip") {
+            return controller_->BackFlip();
+        } else if (cmd == "HandStand") {
+            return controller_->HandStand(message["param"]["flag"]);
+        } else if (cmd == "FreeWalk") {
+            return controller_->FreeWalk();
+        } else if (cmd == "FreeBound") {
+            return controller_->FreeBound(message["param"]["flag"]);
+        } else if (cmd == "FreeJump") {
+            return controller_->FreeJump(message["param"]["flag"]);
+        } else if (cmd == "FreeAvoid") {
+            return controller_->FreeAvoid(message["param"]["flag"]);
+        } else if (cmd == "ClassicWalk") {
+            return controller_->ClassicWalk(message["param"]["flag"]);
+        } else if (cmd == "WalkUpright") {
+            return controller_->WalkUpright(message["param"]["flag"]);
+        } else if (cmd == "CrossStep") {
+            return controller_->CrossStep(message["param"]["flag"]);
+        } else if (cmd == "AutoRecoverSet") {
+            return controller_->AutoRecoverSet(message["param"]["flag"]);
+        } else if (cmd == "StaticWalk") {
+            return controller_->StaticWalk();
+        } else if (cmd == "TrotRun") {
+            return controller_->TrotRun();
+        } else if (cmd == "EconomicGait") {
+            return controller_->EconomicGait();
+        } else if (cmd == "SwitchAvoidMode") {
+            return controller_->SwitchAvoidMode();
+        }
+        else if (cmd == "BodyHeight") {
+            return controller_->BodyHeight(message["param"]["height"]);
+        } else if (cmd == "SwitchGait") {
+            return controller_->SwitchGait(message["param"]["gait"]);
+        } else if (cmd == "TrajectoryFollow") {
+            return controller_->TrajectoryFollow(message["param"]["path"]);
+        } else if (cmd == "ContinuousGait") {
+            return controller_->ContinuousGait(message["param"]["flag"]);
+        } else if (cmd == "MoveToPos") {
+            return controller_->MoveToPos(message["param"]["x"], message["param"]["y"], message["param"]["yaw"]);
+        } else if (cmd == "FastWalk") {
+            return controller_->FastWalk(message["param"]["flag"]);
+        } else if (cmd == "FootRaiseHeight") {
+            return controller_->FootRaiseHeight(message["param"]["height"]);
+        }
+        else {
            LOG_ERROR("Unknown Sport command: " + cmd);
            return false;
        }
--- a/src/navigation.cpp
+++ b/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);
+}