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.

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_;