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

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

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