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

2025-09-07 16:43:22 +08:00
parent b2b6ddf245
commit 6acff483dd
16 changed files with 3429 additions and 1 deletions
--- a/include/config.hpp
+++ b/include/config.hpp
@@ -0,0 +1,192 @@
+#pragma once
+
+#include <string>
+#include <nlohmann/json.hpp>
+
+namespace custom {
+
+// Compile-time configuration flag
+constexpr bool USE_COMPILE_TIME_CONFIG = true;
+
+// Network settings
+constexpr const char* NETWORK_INTERFACE = "eth0";
+
+// MQTT settings
+constexpr const char* MQTT_BROKER = "localhost";
+constexpr int MQTT_PORT = 1883;
+constexpr const char* MQTT_USERNAME = "";
+constexpr const char* MQTT_PASSWORD = "";
+constexpr const char* MQTT_CLIENT_ID = "unitree_go2_client";
+
+// Topic settings
+constexpr const char* TOPIC_PREFIX = "unitree/go2";
+constexpr const char* TOPIC_CMD = "cmd";
+constexpr const char* TOPIC_STATE = "state";
+constexpr const char* TOPIC_VIDEO = "video";
+constexpr const char* TOPIC_AUDIO = "audio";
+
+// Robot control settings
+constexpr double CONTROL_FREQUENCY = 200.0; // Hz
+constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz
+constexpr bool ENABLE_VIDEO = true;
+constexpr bool ENABLE_AUDIO = true;
+
+// Safety settings
+constexpr double MAX_LINEAR_VELOCITY = 1.5; // m/s
+constexpr double MAX_ANGULAR_VELOCITY = 2.0; // rad/s
+constexpr double EMERGENCY_STOP_TIMEOUT = 5.0; // seconds
+
+// Motion settings
+constexpr double STAND_HEIGHT = 0.0; // relative height
+constexpr int GAIT = 0; // 0:idle, 1:trot, 2:trot_running
+
+// Video settings
+constexpr int VIDEO_WIDTH = 1280;
+constexpr int VIDEO_HEIGHT = 720;
+constexpr int VIDEO_FPS = 30;
+constexpr const char* VIDEO_FORMAT = "h264";
+
+// Audio settings
+constexpr int AUDIO_SAMPLE_RATE = 16000;
+constexpr int AUDIO_CHANNELS = 1;
+constexpr const char* AUDIO_FORMAT = "pcm";
+
+// Configuration presets
+struct HighPerformancePreset {
+    static constexpr double CONTROL_FREQUENCY = 500.0;
+    static constexpr double STATE_PUBLISH_FREQUENCY = 100.0;
+    static constexpr double MAX_LINEAR_VELOCITY = 2.5;
+    static constexpr double MAX_ANGULAR_VELOCITY = 3.0;
+    static constexpr bool ENABLE_VIDEO = false; // Disable for performance
+    static constexpr bool ENABLE_AUDIO = false; // Disable for performance
+};
+
+struct DevelopmentPreset {
+    static constexpr double CONTROL_FREQUENCY = 100.0;
+    static constexpr double STATE_PUBLISH_FREQUENCY = 20.0;
+    static constexpr double MAX_LINEAR_VELOCITY = 0.8;
+    static constexpr double MAX_ANGULAR_VELOCITY = 1.0;
+    static constexpr bool ENABLE_VIDEO = true;
+    static constexpr bool ENABLE_AUDIO = true;
+};
+
+struct SafetyPreset {
+    static constexpr double CONTROL_FREQUENCY = 50.0;
+    static constexpr double STATE_PUBLISH_FREQUENCY = 10.0;
+    static constexpr double MAX_LINEAR_VELOCITY = 0.5;
+    static constexpr double MAX_ANGULAR_VELOCITY = 0.5;
+    static constexpr double EMERGENCY_STOP_TIMEOUT = 2.0;
+    static constexpr bool ENABLE_VIDEO = true;
+    static constexpr bool ENABLE_AUDIO = true;
+};
+
+// Runtime configuration structure - uses compile-time defaults
+struct RobotConfig {
+    // Network settings
+    std::string network_interface = NETWORK_INTERFACE;
+    
+    // MQTT settings
+    std::string mqtt_broker = MQTT_BROKER;
+    int mqtt_port = MQTT_PORT;
+    std::string mqtt_username = MQTT_USERNAME;
+    std::string mqtt_password = MQTT_PASSWORD;
+    std::string mqtt_client_id = MQTT_CLIENT_ID;
+    
+    // Topics
+    std::string topic_prefix = TOPIC_PREFIX;
+    std::string cmd_topic = TOPIC_CMD;
+    std::string state_topic = TOPIC_STATE;
+    std::string video_topic = TOPIC_VIDEO;
+    std::string audio_topic = TOPIC_AUDIO;
+    
+    // Robot settings
+    double control_frequency = CONTROL_FREQUENCY;
+    double state_publish_frequency = STATE_PUBLISH_FREQUENCY;
+    bool enable_video = ENABLE_VIDEO;
+    bool enable_audio = ENABLE_AUDIO;
+    
+    // Safety settings
+    double max_linear_velocity = MAX_LINEAR_VELOCITY;
+    double max_angular_velocity = MAX_ANGULAR_VELOCITY;
+    double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
+    
+    // Motion settings
+    double stand_height = STAND_HEIGHT;
+    double default_gait = GAIT;
+    
+    // Video settings
+    int video_width = VIDEO_WIDTH;
+    int video_height = VIDEO_HEIGHT;
+    int video_fps = VIDEO_FPS;
+    std::string video_format = VIDEO_FORMAT;
+    
+    // Audio settings
+    int audio_sample_rate = AUDIO_SAMPLE_RATE;
+    int audio_channels = AUDIO_CHANNELS;
+    std::string audio_format = AUDIO_FORMAT;
+};
+
+class Config {
+public:
+    static Config& getInstance();
+    
+    /**
+     * Load configuration from JSON file
+     */
+    bool loadConfig(const std::string& configPath);
+    
+    /**
+     * Save current configuration to JSON file
+     */
+    bool saveConfig(const std::string& configPath);
+    
+    /**
+     * Load configuration from compile-time defaults
+     */
+    void loadDefaults();
+    
+    /**
+     * Load high-performance preset
+     */
+    void loadHighPerformancePreset();
+    
+    /**
+     * Load development preset
+     */
+    void loadDevelopmentPreset();
+    
+    /**
+     * Load safety preset
+     */
+    void loadSafetyPreset();
+    
+    /**
+     * Validate configuration values
+     */
+    bool validateConfig();
+    
+    /**
+     * Get current configuration
+     */
+    const RobotConfig& getConfig() const { return config_; }
+    
+    /**
+     * Get mutable reference to configuration
+     */
+    RobotConfig& getConfig() { return config_; }
+    
+    /**
+     * Convenience getters
+     */
+    std::string getFullTopic(const std::string& topic) const;
+
+private:
+    Config();
+    ~Config() = default;
+    Config(const Config&) = delete;
+    Config& operator=(const Config&) = delete;
+    
+    RobotConfig config_;
+};
+
+} // namespace custom
--- a/include/controller.hpp
+++ b/include/controller.hpp
@@ -0,0 +1,233 @@
+#pragma once
+
+#include <memory>
+#include <thread>
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+#include <chrono>
+#include <nlohmann/json.hpp>
+
+#include <unitree/robot/go2/sport/sport_client.hpp>
+#include <unitree/robot/go2/obstacles_avoid/obstacles_avoid_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 <unitree/idl/go2/LowState_.hpp>
+
+namespace custom {
+
+enum class RobotMode {
+    IDLE,
+    SPORT,
+    LOW_LEVEL,
+    RECOVERY
+};
+
+enum class MotionState {
+    STOPPED,
+    STANDING,
+    WALKING,
+    RUNNING,
+    SITTING,
+    LYING,
+    DANCING,
+    SPECIAL_MOTION
+};
+
+struct RobotState {
+    RobotMode mode = RobotMode::IDLE;
+    MotionState motion_state = MotionState::STOPPED;
+    
+    // Position and orientation
+    double position[3] = {0.0, 0.0, 0.0}; // x, y, z
+    double orientation[3] = {0.0, 0.0, 0.0}; // roll, pitch, yaw
+    double velocity[3] = {0.0, 0.0, 0.0}; // vx, vy, vyaw
+    
+    // IMU data
+    double imu_acc[3] = {0.0, 0.0, 0.0};
+    double imu_gyro[3] = {0.0, 0.0, 0.0};
+    double imu_quat[4] = {1.0, 0.0, 0.0, 0.0};
+    
+    // Battery and system
+    double battery_voltage = 0.0;
+    double battery_current = 0.0;
+    int battery_percentage = 0;
+    double temperature = 0.0;
+    
+    // Motor states (20 motors)
+    double motor_positions[20] = {0};
+    double motor_velocities[20] = {0};
+    double motor_torques[20] = {0};
+    double motor_temperatures[20] = {0};
+    
+    // Status flags
+    bool is_connected = false;
+    bool emergency_stop = false;
+    bool low_battery = false;
+    bool overheated = false;
+    
+    std::chrono::steady_clock::time_point timestamp;
+};
+
+struct MotionCommand {
+    enum Type {
+        VELOCITY,
+        POSITION,
+        SPECIAL_ACTION,
+        GAIT_CHANGE,
+        BODY_POSE
+    } type;
+    
+    // Velocity command
+    double linear_velocity[3] = {0.0, 0.0, 0.0}; // vx, vy, vz
+    double angular_velocity[3] = {0.0, 0.0, 0.0}; // wx, wy, wz
+    
+    // Position command
+    double target_position[3] = {0.0, 0.0, 0.0};
+    double target_orientation[3] = {0.0, 0.0, 0.0};
+    
+    // Body pose
+    double body_height = 0.0;
+    double body_roll = 0.0;
+    double body_pitch = 0.0;
+    double body_yaw = 0.0;
+    
+    // Special actions
+    std::string action_name;
+    nlohmann::json action_params;
+    
+    // Gait parameters
+    int gait_type = 0;
+    double step_height = 0.0;
+    
+    std::chrono::steady_clock::time_point timestamp;
+    double duration = 0.0; // command duration in seconds
+};
+
+class Controller {
+public:
+    using StateCallback = std::function<void(const RobotState&)>;
+    using ErrorCallback = std::function<void(const std::string&)>;
+
+    explicit Controller(const std::string& networkInterface);
+    ~Controller();
+
+    bool initialize();
+    void shutdown();
+
+    // State management
+    bool start();
+    bool stop();
+    bool isRunning() const { return running_; }
+    
+    RobotState getCurrentState() const;
+    void setStateCallback(StateCallback callback);
+    void setErrorCallback(ErrorCallback callback);
+
+    // Motion control
+    bool executeCommand(const MotionCommand& command);
+    bool emergencyStop();
+    bool recoveryStand();
+    
+    // Basic motions
+    bool standUp();
+    bool standDown();
+    bool sit();
+    bool lie();
+    bool damp();
+    
+    // Movement
+    bool move(double vx, double vy, double vyaw);
+    bool moveToPosition(double x, double y, double yaw);
+    bool stop();
+    
+    // Body control
+    bool setBodyPose(double roll, double pitch, double yaw, double height);
+    bool balanceStand();
+    
+    // Gait control
+    bool switchGait(int gaitType);
+    bool setSpeedLevel(int level);
+    
+    // Special actions
+    bool performAction(const std::string& actionName, const nlohmann::json& params = {});
+    
+    // Dance and tricks
+    bool dance1();
+    bool dance2();
+    bool frontFlip();
+    bool backFlip();
+    bool hello();
+    bool stretch();
+    
+    // Obstacle avoidance
+    bool enableObstacleAvoidance(bool enable);
+    bool isObstacleAvoidanceEnabled();
+    
+    // Video control
+    bool startVideo();
+    bool stopVideo();
+    bool isVideoActive();
+    
+    // Audio control
+    bool playAudio(const std::string& text);
+    bool stopAudio();
+
+private:
+    void controlLoop();
+    void stateUpdateLoop();
+    void processCommands();
+    
+    void updateSportModeState(const void* message);
+    void updateLowLevelState(const void* message);
+    
+    bool validateCommand(const MotionCommand& command);
+    void executeMotionCommand(const MotionCommand& command);
+    
+    void handleError(const std::string& error);
+    void checkSafetyLimits();
+    
+    // SDK clients
+    std::unique_ptr<unitree::robot::go2::SportClient> sportClient_;
+    std::unique_ptr<unitree::robot::go2::ObstaclesAvoidClient> obstacleClient_;
+    std::unique_ptr<unitree::robot::go2::VideoClient> videoClient_;
+    std::unique_ptr<unitree::robot::go2::VuiClient> vuiClient_;
+    
+    // State subscribers
+    std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>> sportStateSubscriber_;
+    std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>> lowStateSubscriber_;
+    
+    // Current state
+    mutable std::mutex stateMutex_;
+    RobotState currentState_;
+    
+    // Command queue
+    std::queue<MotionCommand> commandQueue_;
+    std::mutex commandMutex_;
+    std::condition_variable commandCondition_;
+    
+    // Control threads
+    std::thread controlThread_;
+    std::thread stateThread_;
+    std::atomic<bool> running_;
+    std::atomic<bool> initialized_;
+    
+    // Callbacks
+    StateCallback stateCallback_;
+    ErrorCallback errorCallback_;
+    
+    // Safety
+    std::chrono::steady_clock::time_point lastCommandTime_;
+    std::atomic<bool> emergencyStopActive_;
+    
+    // Network
+    std::string networkInterface_;
+    
+    // Timing
+    std::chrono::milliseconds controlPeriod_;
+    std::chrono::milliseconds statePeriod_;
+};
+
+} // namespace custom
--- a/include/custom_robot.hpp
+++ b/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
--- a/include/logger.hpp
+++ b/include/logger.hpp
@@ -0,0 +1,53 @@
+#pragma once
+
+#include <string>
+#include <fstream>
+#include <memory>
+#include <mutex>
+#include <sstream>
+#include <chrono>
+#include <iomanip>
+
+namespace custom {
+
+enum class LogLevel {
+    DEBUG = 0,
+    INFO = 1,
+    WARN = 2,
+    ERROR = 3
+};
+
+class Logger {
+public:
+    static Logger& getInstance();
+    
+    void setLevel(LogLevel level);
+    void setLogFile(const std::string& filename);
+    
+    void log(LogLevel level, const std::string& message);
+    void debug(const std::string& message);
+    void info(const std::string& message);
+    void warn(const std::string& message);
+    void error(const std::string& message);
+
+private:
+    Logger() = default;
+    ~Logger() = default;
+    Logger(const Logger&) = delete;
+    Logger& operator=(const Logger&) = delete;
+    
+    std::string getCurrentTime();
+    std::string levelToString(LogLevel level);
+    
+    LogLevel currentLevel_ = LogLevel::INFO;
+    std::unique_ptr<std::ofstream> logFile_;
+    std::mutex logMutex_;
+};
+
+// Convenience macros
+#define LOG_DEBUG(msg) Logger::getInstance().debug(msg)
+#define LOG_INFO(msg) Logger::getInstance().info(msg)
+#define LOG_WARN(msg) Logger::getInstance().warn(msg)
+#define LOG_ERROR(msg) Logger::getInstance().error(msg)
+
+} // namespace custom
--- a/include/mqtt.hpp
+++ b/include/mqtt.hpp
@@ -0,0 +1,97 @@
+#pragma once
+
+#include <string>
+#include <functional>
+#include <memory>
+#include <thread>
+#include <atomic>
+#include <queue>
+#include <mutex>
+#include <condition_variable>
+#include <nlohmann/json.hpp>
+#include <mqtt/async_client.h>
+
+namespace custom {
+
+class MqttClient {
+public:
+    using MessageCallback = std::function<void(const std::string& topic, const std::string& payload)>;
+    using ConnectionCallback = std::function<void(bool connected)>;
+
+    MqttClient(const std::string& broker, int port, const std::string& clientId);
+    ~MqttClient();
+
+    bool connect(const std::string& username = "", const std::string& password = "");
+    void disconnect();
+    bool isConnected() const;
+
+    bool subscribe(const std::string& topic, int qos = 1);
+    bool unsubscribe(const std::string& topic);
+    
+    bool publish(const std::string& topic, const std::string& payload, int qos = 1, bool retain = false);
+    bool publishJson(const std::string& topic, const nlohmann::json& json, int qos = 1, bool retain = false);
+
+    void setMessageCallback(MessageCallback callback);
+    void setConnectionCallback(ConnectionCallback callback);
+
+    // Message queue for async processing
+    void startMessageProcessor();
+    void stopMessageProcessor();
+
+private:
+    class Callback : public virtual mqtt::callback,
+                     public virtual mqtt::iaction_listener {
+    public:
+        Callback(MqttClient* client) : client_(client) {}
+
+        void connected(const std::string& cause) override;
+        void connection_lost(const std::string& cause) override;
+        void message_arrived(mqtt::const_message_ptr msg) override;
+        void delivery_complete(mqtt::delivery_token_ptr token) override;
+
+        void on_failure(const mqtt::token& tok) override;
+        void on_success(const mqtt::token& tok) override;
+
+    private:
+        MqttClient* client_;
+    };
+
+    struct QueuedMessage {
+        std::string topic;
+        std::string payload;
+    };
+
+    void processMessageQueue();
+    void handleConnectionLost();
+    void handleConnectionEstablished();
+
+    std::unique_ptr<mqtt::async_client> client_;
+    std::unique_ptr<Callback> callback_;
+    
+    MessageCallback messageCallback_;
+    ConnectionCallback connectionCallback_;
+    
+    std::atomic<bool> connected_;
+    std::atomic<bool> reconnecting_;
+    
+    // Message processing
+    std::queue<QueuedMessage> messageQueue_;
+    std::mutex queueMutex_;
+    std::condition_variable queueCondition_;
+    std::thread messageProcessor_;
+    std::atomic<bool> processorRunning_;
+    
+    // Connection parameters
+    std::string broker_;
+    int port_;
+    std::string clientId_;
+    std::string username_;
+    std::string password_;
+    
+    // Reconnection
+    std::thread reconnectThread_;
+    std::atomic<bool> shouldReconnect_;
+    int reconnectDelay_ = 5; // seconds
+};
+
+} // namespace custom