diff --git a/CMakeLists.txt b/CMakeLists.txt
index d69e634..8a5625d 100644
--- 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
diff --git a/include/controller.hpp b/include/controller.hpp
index 5f4fd29..b757025 100644
--- 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 {
 
@@ -44,11 +46,25 @@ public:
     bool UseRemoteCommandFromApi(bool isRemoteCommandsFromApi);
     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_;
diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp
index 0e1f535..cf29fe6 100644
--- 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_;
     
diff --git a/include/navigation.hpp b/include/navigation.hpp
new file mode 100644
index 0000000..61a3f0b
--- /dev/null
+++ 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
diff --git a/src/controller.cpp b/src/controller.cpp
index b338d46..7ba4790 100644
--- 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
\ No newline at end of file
diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp
index fa205a4..1923e26 100644
--- 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 {
@@ -41,6 +41,10 @@ CustomRobot::~CustomRobot() {
     if (rsc_) {
         rsc_.reset();
     }
+
+    if (navigation_) {
+        navigation_.reset();
+    }
     
     try {
         unitree::robot::ChannelFactory::Instance()->Release();
@@ -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;
         }
diff --git a/src/navigation.cpp b/src/navigation.cpp
new file mode 100644
index 0000000..9c52702
--- /dev/null
+++ 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);
+}