Add initial project structure with CMake configuration, source files, and README documentation for Unitree GO2 Custom Controller

include/custom_robot.hpp

@@ -0,0 +1,123 @@
+#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>
+
+namespace custom {
+
+class CustomRobot {
+public:
+    explicit CustomRobot(const std::string& configFile = "");
+    ~CustomRobot();
+
+    bool initialize();
+    void shutdown();
+    
+    bool start();
+    bool stop();
+    bool isRunning() const { return running_; }
+    
+    // Main run loop
+    void run();
+    void runAsync();
+    void waitForShutdown();
+
+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);
+    
+    // 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