Remove MQTT dependency and refactor configuration structure. Updated CMakeLists.txt to eliminate MQTT library checks and adjusted config.hpp to use string_view for configuration parameters. Simplified controller and custom_robot classes by removing MQTT-related code and enhancing robot state management. Introduced service management methods in CustomRobot for better state handling.

include/custom_robot.hpp

@@ -1,123 +1,34 @@
 #pragma once
 
 #include "controller.hpp"
-#include "mqtt.hpp"
 #include "config.hpp"
 #include "logger.hpp"
 
 #include <memory>
-#include <thread>
 #include <atomic>
-#include <chrono>
-#include <nlohmann/json.hpp>
+#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
 
 namespace custom {
 
 class CustomRobot {
 public:
-    explicit CustomRobot(const std::string& configFile = "");
+    explicit CustomRobot();
     ~CustomRobot();
 
     bool initialize();
-    void shutdown();
-    
     bool start();
-    bool stop();
-    bool isRunning() const { return running_; }
     
-    // Main run loop
-    void run();
-    void runAsync();
-    void waitForShutdown();
+    // Robot state methods
+    bool GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList);
+    bool SwitchService(const std::string& serviceName, bool enable);
+    bool SetReportFreq(int32_t interval, int32_t duration);
 
 private:
-    // MQTT message handlers
-    void handleMqttMessage(const std::string& topic, const std::string& payload);
-    void handleCommandMessage(const nlohmann::json& command);
-    void handleConfigMessage(const nlohmann::json& config);
-    void handleControlMessage(const nlohmann::json& control);
+    std::unique_ptr<Controller> controller_;
+    std::unique_ptr<unitree::robot::go2::RobotStateClient> stateClient_;
     
-    // Robot state handlers
-    void handleRobotState(const RobotState& state);
-    void handleRobotError(const std::string& error);
-    
-    // Publishing functions
-    void publishState();
-    void publishHeartbeat();
-    void publishError(const std::string& error);
-    void publishResponse(const std::string& requestId, bool success, const std::string& message = "");
-    
-    // Command processing
-    bool processMotionCommand(const nlohmann::json& cmd);
-    bool processSpecialAction(const nlohmann::json& cmd);
-    bool processSystemCommand(const nlohmann::json& cmd);
-    bool processConfigCommand(const nlohmann::json& cmd);
-    
-    // Utility functions
-    MotionCommand jsonToMotionCommand(const nlohmann::json& json);
-    nlohmann::json robotStateToJson(const RobotState& state);
-    nlohmann::json createStatusMessage();
-    
-    void startPeriodicTasks();
-    void stopPeriodicTasks();
-    
-    // Connection management
-    void handleMqttConnection(bool connected);
-    void reconnectMqtt();
-    
-    // Safety monitoring
-    void safetyMonitorLoop();
-    void checkEmergencyConditions();
-    
-    // Components
-    std::unique_ptr<Controller> robotController_;
-    std::unique_ptr<MqttClient> mqttClient_;
-    
-    // Configuration
-    std::string configFile_;
-    
-    // State
     std::atomic<bool> running_;
     std::atomic<bool> initialized_;
-    std::atomic<bool> mqttConnected_;
-    
-    // Threads
-    std::thread mainThread_;
-    std::thread safetyThread_;
-    std::vector<std::thread> periodicThreads_;
-    
-    // Timing
-    std::chrono::steady_clock::time_point lastHeartbeat_;
-    std::chrono::steady_clock::time_point lastStatePublish_;
-    std::chrono::steady_clock::time_point lastCommandReceived_;
-    
-    // Statistics
-    struct Statistics {
-        uint64_t commandsReceived = 0;
-        uint64_t commandsExecuted = 0;
-        uint64_t commandsFailed = 0;
-        uint64_t statesPublished = 0;
-        uint64_t errorsOccurred = 0;
-        std::chrono::steady_clock::time_point startTime;
-    } stats_;
-    
-    // Safety parameters
-    double maxIdleTime_ = 30.0; // seconds
-    double heartbeatInterval_ = 5.0; // seconds
-    double statePublishInterval_ = 0.02; // 50Hz
-    
-    // Current robot state cache
-    mutable std::mutex stateCacheMutex_;
-    RobotState lastKnownState_;
-    
-    // Request tracking
-    std::mutex requestMutex_;
-    std::map<std::string, std::chrono::steady_clock::time_point> pendingRequests_;
-    
-    // Error handling
-    std::mutex errorMutex_;
-    std::queue<std::string> errorQueue_;
-    void processErrorQueue();
 };
 
 } // namespace custom