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

.gitignore

@@ -0,0 +1,76 @@
+# Build directories
+build/
+bin/
+lib/
+*.so
+*.a
+
+# CMake
+CMakeCache.txt
+CMakeFiles/
+cmake_install.cmake
+Makefile
+*.cmake
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+unitree_go2_custom
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb
+
+# Log files
+*.log
+logs/
+
+# Configuration backups
+*.json.bak
+*.json.old
+
+# IDE files
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+
+# OS files
+.DS_Store
+Thumbs.db
+
+# Temporary files
+*.tmp
+*.temp
+
+# Core dumps
+core
+core.*

CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.16)
+project(unitree_go2_custom VERSION 1.0.0)
+
+# Set C++ standard
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Find required packages
+find_package(PkgConfig REQUIRED)
+find_package(Threads REQUIRED)
+
+# Find unitree_sdk2
+set(UNITREE_SDK2_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../unitree_sdk2")
+if(NOT EXISTS ${UNITREE_SDK2_PATH})
+    message(FATAL_ERROR "unitree_sdk2 not found at ${UNITREE_SDK2_PATH}")
+endif()
+
+# Add unitree_sdk2 as subdirectory
+add_subdirectory(${UNITREE_SDK2_PATH} unitree_sdk2)
+
+# Find MQTT library (Eclipse Paho)
+pkg_check_modules(PAHO_MQTT REQUIRED libpaho-mqtt3c)
+pkg_check_modules(PAHO_MQTTPP REQUIRED libpaho-mqttpp3)
+
+# Find JSON library
+find_package(nlohmann_json REQUIRED)
+
+# Include directories
+include_directories(
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+    ${UNITREE_SDK2_PATH}/include
+)
+
+# Source files
+set(SOURCES
+    src/main.cpp
+    src/mqtt.cpp
+    src/controller.cpp
+    src/custom_robot.cpp
+    src/config.cpp
+    src/logger.cpp
+)
+
+# Create executable with simple name
+add_executable(main ${SOURCES})
+
+# Link libraries
+target_link_libraries(main
+    unitree_sdk2
+    ${PAHO_MQTT_LIBRARIES}
+    ${PAHO_MQTTPP_LIBRARIES}
+    nlohmann_json::nlohmann_json
+    Threads::Threads
+)
+
+# Compiler options
+target_compile_options(main PRIVATE
+    ${PAHO_MQTT_CFLAGS_OTHER}
+    ${PAHO_MQTTPP_CFLAGS_OTHER}
+)
+
+# Set output directory
+set_target_properties(main PROPERTIES
+    RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
+# Install target
+install(TARGETS main
+    RUNTIME DESTINATION bin
+)
+
+# Copy config files
+install(FILES config/robot_config.json
+    DESTINATION etc/${PROJECT_NAME}
+)

README.md

@@ -1,2 +1,361 @@
-# unitree-go2
+# Unitree GO2 Custom Controller
 
+A high-performance C++ implementation for controlling Unitree GO2 robot with real-time MQTT communication. This project provides a robust, modular architecture for robot control with comprehensive safety features and flexible configuration management.
+
+## Features
+
+- **🤖 Robot Control**: Full integration with Unitree SDK2 for GO2 robot control
+- **📡 MQTT Communication**: Real-time command and state communication via MQTT
+- **🏗️ Modular Architecture**: Clean separation of concerns with well-defined interfaces
+- **🛡️ Safety Systems**: Emergency stop, timeout handling, and safety limits
+- **📝 Comprehensive Logging**: Multi-level logging with file and console output
+- **⚙️ Configuration Management**: JSON-based configuration with runtime presets
+- **📊 State Publishing**: Real-time robot state broadcasting
+- **🎭 Special Actions**: Support for dances, tricks, and custom motions
+- **🚀 High Performance**: Optimized for real-time control with configurable frequencies
+- **🔧 Development Tools**: Built-in scripts and utilities for easy deployment
+
+## Architecture
+
+The system consists of several key components:
+
+- **CustomRobot**: Main orchestrator class handling MQTT and robot coordination
+- **Controller**: Direct interface to Unitree SDK2 for robot control
+- **MqttClient**: Asynchronous MQTT client with reconnection and message queuing
+- **Config**: Configuration loading, validation, and management
+- **Logger**: Thread-safe logging system with multiple output targets
+
+## Dependencies
+
+### Required System Packages
+
+```bash
+# Ubuntu/Debian
+sudo apt update
+sudo apt install -y \
+    build-essential \
+    cmake \
+    pkg-config \
+    libpaho-mqtt-dev \
+    libpaho-mqttpp-dev \
+    nlohmann-json3-dev \
+    libssl-dev
+
+# Or build from source if packages not available
+```
+
+### Unitree SDK2
+
+This project requires the Unitree SDK2 to be available at `../unitree_sdk2` relative to this directory.
+
+## Quick Start
+
+```bash
+# Create build directory
+mkdir build && cd build
+
+# Configure with CMake
+cmake ..
+
+# Build the project
+make -j$(nproc)
+
+# Run the robot (that's it!)
+./main
+```
+
+The robot will start immediately with default settings and be ready to receive MQTT commands!
+
+## Configuration
+
+The system supports flexible configuration through JSON files and built-in presets. Configuration files are optional - the system can run with compile-time defaults.
+
+### Configuration File Structure
+
+Create `config/robot_config.json` for custom settings:
+
+```json
+{
+  "network": {
+    "interface": "eth0"
+  },
+  "mqtt": {
+    "broker": "localhost",
+    "port": 1883,
+    "username": "",
+    "password": "",
+    "client_id": "unitree_go2_client",
+    "keep_alive": 60,
+    "qos": 1
+  },
+  "control": {
+    "frequency": 200,
+    "state_publish_frequency": 50
+  },
+  "safety": {
+    "max_linear_velocity": 1.5,
+    "max_angular_velocity": 2.0,
+    "emergency_stop_timeout": 5.0,
+    "command_timeout": 2.0
+  },
+  "logging": {
+    "level": "INFO",
+    "file_output": false,
+    "console_output": true
+  }
+}
+```
+
+### Using Configuration Presets
+
+Instead of creating JSON files, you can use built-in presets:
+
+```bash
+# High performance mode
+./build/unitree_go2_custom --preset high-perf
+
+# Development mode
+./build/unitree_go2_custom --preset dev
+
+# Safety-first mode
+./build/unitree_go2_custom --preset safety
+```
+
+## Usage
+
+### Basic Usage
+
+```bash
+# Simple execution - just run it!
+./build/main
+
+# That's it! No parameters needed.
+# The robot will start with default settings and be ready to receive MQTT commands.
+```
+
+### Default Configuration
+
+The robot runs with sensible defaults out of the box:
+
+- **Network Interface**: `eth0` (auto-detected)
+- **MQTT Broker**: `localhost:1883`
+- **Control Frequency**: 200Hz
+- **Safety Limits**: Conservative settings for safe operation
+- **Logging**: INFO level to console
+
+No configuration files or command-line arguments needed!
+
+## MQTT API
+
+The robot communicates via MQTT using the following topic structure:
+
+### Command Topics
+
+- `unitree/go2/cmd/motion`: Motion commands
+- `unitree/go2/cmd/control`: System control commands
+- `unitree/go2/cmd/config`: Configuration updates
+
+### State Topics
+
+- `unitree/go2/state/robot`: Robot state (position, IMU, motors, etc.)
+- `unitree/go2/state/heartbeat`: System status and statistics
+- `unitree/go2/state/response`: Command execution responses
+- `unitree/go2/state/error`: Error messages
+
+### Motion Commands
+
+```json
+{
+  "request_id": "unique_id",
+  "type": "motion",
+  "motion_type": "velocity",
+  "linear_velocity": [1.0, 0.0, 0.0],
+  "angular_velocity": [0.0, 0.0, 0.5],
+  "duration": 2.0
+}
+```
+
+### Special Actions
+
+```json
+{
+  "request_id": "unique_id",
+  "type": "special_action",
+  "action": "dance1",
+  "params": {}
+}
+```
+
+### System Commands
+
+```json
+{
+  "request_id": "unique_id",
+  "type": "system",
+  "command": "emergency_stop"
+}
+```
+
+## Supported Actions
+
+### Basic Motions
+- `stand_up`: Stand up from lying position
+- `stand_down`: Lie down from standing position
+- `sit`: Sit down
+- `damp`: Enable damping mode
+- `balance_stand`: Balanced standing with pose control
+- `recovery_stand`: Recovery from emergency stop
+
+### Special Actions
+- `dance1`, `dance2`: Dance routines
+- `hello`: Greeting gesture
+- `stretch`: Stretching motion
+- `front_flip`, `back_flip`: Acrobatic flips (use with caution)
+
+### Movement
+- Velocity control: Linear and angular velocity commands
+- Position control: Move to absolute positions
+- Body pose control: Roll, pitch, yaw, and height adjustment
+
+## Safety Features
+
+- **Emergency Stop**: Immediate motion halt and damping activation
+- **Velocity Limits**: Configurable maximum linear and angular velocities
+- **Command Timeout**: Automatic stop if no commands received within timeout
+- **Battery Monitoring**: Low battery detection and safe shutdown
+- **Connection Monitoring**: Automatic reconnection and error handling
+
+## Development
+
+### Adding New Actions
+
+1. Add action handler in `Controller::performAction()`
+2. Update MQTT command processing in `CustomRobot::processSpecialAction()`
+3. Add action documentation
+
+### Extending MQTT API
+
+1. Define new message types in the appropriate handler functions
+2. Update JSON parsing and validation
+3. Add corresponding response handling
+
+### Custom Configurations
+
+Create specialized configuration files for different environments:
+
+```bash
+# Development configuration
+cp config/robot_config.json config/dev_config.json
+
+# Production configuration  
+cp config/robot_config.json config/prod_config.json
+```
+
+## Quick Start Scripts
+
+The project includes utility scripts for easy deployment:
+
+```bash
+# Install system dependencies
+./scripts/install_deps.sh
+
+# Run robot with recommended settings
+./scripts/run_robot.sh
+```
+
+## Project Structure
+
+```
+unitree-go2/
+├── CMakeLists.txt          # Build configuration
+├── README.md               # This file
+├── LICENSE                 # License information
+├── config/                 # Configuration files (optional)
+├── include/                # Header files
+│   ├── config.hpp          # Configuration management
+│   ├── controller.hpp      # Robot controller
+│   ├── custom_robot.hpp    # Main orchestrator
+│   ├── logger.hpp          # Logging system
+│   └── mqtt.hpp            # MQTT client
+├── src/                    # Source files
+│   ├── config.cpp
+│   ├── controller.cpp
+│   ├── custom_robot.cpp
+│   ├── logger.cpp
+│   ├── main.cpp
+│   └── mqtt.cpp
+└── scripts/                # Utility scripts
+    ├── install_deps.sh     # Install dependencies
+    └── run_robot.sh        # Run with optimal settings
+```
+
+## Troubleshooting
+
+### Common Issues
+
+1. **SDK Not Found**: Ensure unitree_sdk2 is in the correct relative path (`../unitree_sdk2`)
+2. **MQTT Connection Failed**: Check broker address, port, and credentials
+3. **Robot Not Responding**: Verify network interface and robot connection
+4. **Permission Denied**: Run with appropriate privileges for network access
+5. **Build Errors**: Run `./scripts/install_deps.sh` to install required packages
+
+
+### Network Issues
+
+Test network connectivity to the robot:
+
+```bash
+# Check interface
+ip addr show eth0
+
+# Test robot connectivity (replace with robot IP)
+ping 192.168.123.15
+
+# Check if MQTT broker is accessible
+telnet localhost 1883
+```
+
+### Performance Tuning
+
+For optimal performance:
+
+```bash
+# Use high-performance preset
+./build/unitree_go2_custom --preset high-perf
+
+# Or customize control frequency
+./build/unitree_go2_custom -c config/high_freq_config.json
+```
+
+## License
+
+This project is licensed under the same terms as the original custom_unitree Python implementation.
+
+## Performance Notes
+
+- **Control Frequency**: Default 200Hz, configurable up to 500Hz for high-performance applications
+- **State Publishing**: Default 50Hz, can be adjusted based on network bandwidth
+- **Memory Usage**: Optimized for minimal heap allocations during runtime
+- **CPU Usage**: Multi-threaded design with separate threads for control, MQTT, and state publishing
+
+## Contributing
+
+1. Follow the existing code style and patterns
+2. Add appropriate error handling and logging
+3. Update documentation for new features
+4. Test thoroughly with the actual robot hardware
+5. Use the provided scripts for consistent development environment
+
+## Version History
+
+- **v1.0.0**: Initial release with full MQTT API and safety systems
+- Modular architecture with clean separation of concerns
+- Support for configuration presets and runtime parameter adjustment
+- Comprehensive logging and error handling
+
+## Acknowledgments
+
+- Inspired by Python `custom_unitree` implementations
+- Uses Unitree SDK2 for robot communication
+- Built with Eclipse Paho MQTT C++ library
+- JSON configuration powered by nlohmann/json

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

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

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

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

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

scripts/install_deps.sh

@@ -0,0 +1,53 @@
+#!/bin/bash
+
+# Install dependencies for Unitree GO2 Custom Controller
+# Supports Ubuntu/Debian systems
+
+set -e
+
+echo "Installing dependencies for Unitree GO2 Custom Controller..."
+
+# Update package list
+sudo apt update
+
+# Install build essentials
+echo "Installing build tools..."
+sudo apt install -y \
+    build-essential \
+    cmake \
+    pkg-config \
+    git
+
+# Install MQTT libraries
+echo "Installing MQTT libraries..."
+sudo apt install -y \
+    libpaho-mqtt-dev \
+    libpaho-mqttpp-dev
+
+# Install JSON library
+echo "Installing JSON library..."
+sudo apt install -y nlohmann-json3-dev
+
+# Install SSL libraries (required by MQTT)
+echo "Installing SSL libraries..."
+sudo apt install -y libssl-dev
+
+# Install additional utilities
+echo "Installing utilities..."
+sudo apt install -y \
+    mosquitto-clients \
+    net-tools \
+    iproute2
+
+echo "Dependencies installed successfully!"
+
+# Check if Unitree SDK2 exists
+if [ ! -d "../unitree_sdk2" ]; then
+    echo "WARNING: unitree_sdk2 not found at ../unitree_sdk2"
+    echo "Please ensure the Unitree SDK2 is available at the correct location"
+fi
+
+echo "Setup complete. You can now build the project with:"
+echo "  mkdir build && cd build"
+echo "  cmake .."
+echo "  make -j\$(nproc)"

scripts/run_robot.sh

@@ -0,0 +1,188 @@
+#!/bin/bash
+
+# Run script for Unitree GO2 Custom Controller
+# This script handles common startup scenarios
+
+set -e
+
+# Default values
+CONFIG_FILE="config/robot_config.json"
+INTERFACE="eth0"
+LOG_LEVEL="INFO"
+LOG_FILE=""
+BACKGROUND=false
+
+# Colors for output
+RED='\033[0;31m'
+GREEN='\033[0;32m'
+YELLOW='\033[1;33m'
+NC='\033[0m' # No Color
+
+print_usage() {
+    echo "Usage: $0 [OPTIONS]"
+    echo ""
+    echo "Options:"
+    echo "  -c, --config FILE       Configuration file (default: config/robot_config.json)"
+    echo "  -i, --interface IFACE   Network interface (default: eth0)"
+    echo "  -l, --log-level LEVEL   Log level: DEBUG, INFO, WARN, ERROR (default: INFO)"
+    echo "  -f, --log-file FILE     Log to file"
+    echo "  -b, --background        Run in background"
+    echo "  -h, --help              Show this help"
+    echo ""
+    echo "Examples:"
+    echo "  $0                                    # Run with defaults"
+    echo "  $0 -i wlan0 -l DEBUG                # Use WiFi interface with debug logging"
+    echo "  $0 -c prod_config.json -f robot.log # Use production config and log to file"
+    echo "  $0 -b                                # Run in background"
+}
+
+log_info() {
+    echo -e "${GREEN}[INFO]${NC} $1"
+}
+
+log_warn() {
+    echo -e "${YELLOW}[WARN]${NC} $1"
+}
+
+log_error() {
+    echo -e "${RED}[ERROR]${NC} $1"
+}
+
+# Parse command line arguments
+while [[ $# -gt 0 ]]; do
+    case $1 in
+        -c|--config)
+            CONFIG_FILE="$2"
+            shift 2
+            ;;
+        -i|--interface)
+            INTERFACE="$2"
+            shift 2
+            ;;
+        -l|--log-level)
+            LOG_LEVEL="$2"
+            shift 2
+            ;;
+        -f|--log-file)
+            LOG_FILE="$2"
+            shift 2
+            ;;
+        -b|--background)
+            BACKGROUND=true
+            shift
+            ;;
+        -h|--help)
+            print_usage
+            exit 0
+            ;;
+        *)
+            log_error "Unknown option: $1"
+            print_usage
+            exit 1
+            ;;
+    esac
+done
+
+# Check if executable exists
+EXECUTABLE="./bin/unitree_go2_custom"
+if [ ! -f "$EXECUTABLE" ]; then
+    EXECUTABLE="./build/bin/unitree_go2_custom"
+    if [ ! -f "$EXECUTABLE" ]; then
+        log_error "Executable not found. Please build the project first:"
+        echo "  mkdir build && cd build && cmake .. && make"
+        exit 1
+    fi
+fi
+
+# Check if configuration file exists
+if [ ! -f "$CONFIG_FILE" ]; then
+    log_warn "Configuration file not found: $CONFIG_FILE"
+    log_info "Creating default configuration..."
+    mkdir -p "$(dirname "$CONFIG_FILE")"
+    cat > "$CONFIG_FILE" << 'EOF'
+{
+  "network": {
+    "interface": "eth0"
+  },
+  "mqtt": {
+    "broker": "localhost",
+    "port": 1883,
+    "client_id": "unitree_go2_client"
+  },
+  "topics": {
+    "prefix": "unitree/go2",
+    "cmd": "cmd",
+    "state": "state"
+  },
+  "robot": {
+    "control_frequency": 200.0,
+    "state_publish_frequency": 50.0
+  },
+  "safety": {
+    "max_linear_velocity": 1.5,
+    "max_angular_velocity": 2.0,
+    "emergency_stop_timeout": 5.0
+  }
+}
+EOF
+    log_info "Default configuration created at: $CONFIG_FILE"
+fi
+
+# Check network interface
+if ! ip link show "$INTERFACE" >/dev/null 2>&1; then
+    log_warn "Network interface '$INTERFACE' not found"
+    log_info "Available interfaces:"
+    ip link show | grep -E '^[0-9]+:' | cut -d: -f2 | tr -d ' '
+    log_info "You can specify a different interface with -i option"
+fi
+
+# Build command
+CMD="$EXECUTABLE"
+CMD="$CMD --config $CONFIG_FILE"
+CMD="$CMD --interface $INTERFACE"
+CMD="$CMD --log-level $LOG_LEVEL"
+
+if [ -n "$LOG_FILE" ]; then
+    CMD="$CMD --log-file $LOG_FILE"
+    # Create log directory if needed
+    mkdir -p "$(dirname "$LOG_FILE")"
+fi
+
+# Display startup information
+log_info "Starting Unitree GO2 Custom Controller"
+log_info "Executable: $EXECUTABLE"
+log_info "Configuration: $CONFIG_FILE"
+log_info "Network Interface: $INTERFACE"
+log_info "Log Level: $LOG_LEVEL"
+if [ -n "$LOG_FILE" ]; then
+    log_info "Log File: $LOG_FILE"
+fi
+
+# Setup signal handlers for graceful shutdown
+cleanup() {
+    log_info "Shutting down robot controller..."
+    if [ -n "$ROBOT_PID" ]; then
+        kill -TERM "$ROBOT_PID" 2>/dev/null || true
+        wait "$ROBOT_PID" 2>/dev/null || true
+    fi
+    log_info "Shutdown complete"
+    exit 0
+}
+
+trap cleanup SIGINT SIGTERM
+
+# Run the robot controller
+if [ "$BACKGROUND" = true ]; then
+    log_info "Starting in background mode..."
+    nohup $CMD > robot_output.log 2>&1 &
+    ROBOT_PID=$!
+    log_info "Robot controller started with PID: $ROBOT_PID"
+    log_info "Output is being logged to: robot_output.log"
+    log_info "Use 'kill $ROBOT_PID' to stop the robot"
+else
+    log_info "Starting robot controller..."
+    log_info "Press Ctrl+C to stop"
+    $CMD &
+    ROBOT_PID=$!
+    wait "$ROBOT_PID"
+fi

src/config.cpp

@@ -0,0 +1,316 @@
+#include "config.hpp"
+#include "logger.hpp"
+#include <fstream>
+#include <iostream>
+
+namespace custom {
+
+Config::Config() {
+    // Load default configuration
+    loadDefaults();
+}
+
+Config& Config::getInstance() {
+    static Config instance;
+    return instance;
+}
+
+void Config::loadDefaults() {
+    using namespace config;
+    
+    config_.network_interface = NETWORK_INTERFACE;
+    config_.mqtt_broker = MQTT_BROKER;
+    config_.mqtt_port = MQTT_PORT;
+    config_.mqtt_username = MQTT_USERNAME;
+    config_.mqtt_password = MQTT_PASSWORD;
+    config_.mqtt_client_id = MQTT_CLIENT_ID;
+    
+    config_.topic_prefix = TOPIC_PREFIX;
+    config_.cmd_topic = TOPIC_CMD;
+    config_.state_topic = TOPIC_STATE;
+    config_.video_topic = TOPIC_VIDEO;
+    config_.audio_topic = TOPIC_AUDIO;
+    
+    config_.control_frequency = CONTROL_FREQUENCY;
+    config_.state_publish_frequency = STATE_PUBLISH_FREQUENCY;
+    config_.enable_video = ENABLE_VIDEO;
+    config_.enable_audio = ENABLE_AUDIO;
+    
+    config_.max_linear_velocity = MAX_LINEAR_VELOCITY;
+    config_.max_angular_velocity = MAX_ANGULAR_VELOCITY;
+    config_.emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
+    
+    config_.stand_height = STAND_HEIGHT;
+    config_.default_gait = GAIT;
+    
+    config_.video_width = VIDEO_WIDTH;
+    config_.video_height = VIDEO_HEIGHT;
+    config_.video_fps = VIDEO_FPS;
+    config_.video_format = VIDEO_FORMAT;
+    
+    config_.audio_sample_rate = AUDIO_SAMPLE_RATE;
+    config_.audio_channels = AUDIO_CHANNELS;
+    config_.audio_format = AUDIO_FORMAT;
+}
+
+void Config::loadHighPerformancePreset() {
+    loadDefaults();
+    config_.control_frequency = HighPerformancePreset::CONTROL_FREQUENCY;
+    config_.state_publish_frequency = HighPerformancePreset::STATE_PUBLISH_FREQUENCY;
+    config_.max_linear_velocity = HighPerformancePreset::MAX_LINEAR_VELOCITY;
+    config_.max_angular_velocity = HighPerformancePreset::MAX_ANGULAR_VELOCITY;
+    config_.enable_video = HighPerformancePreset::ENABLE_VIDEO;
+    config_.enable_audio = HighPerformancePreset::ENABLE_AUDIO;
+}
+
+void Config::loadDevelopmentPreset() {
+    loadDefaults();
+    config_.control_frequency = DevelopmentPreset::CONTROL_FREQUENCY;
+    config_.state_publish_frequency = DevelopmentPreset::STATE_PUBLISH_FREQUENCY;
+    config_.max_linear_velocity = DevelopmentPreset::MAX_LINEAR_VELOCITY;
+    config_.max_angular_velocity = DevelopmentPreset::MAX_ANGULAR_VELOCITY;
+    config_.enable_video = DevelopmentPreset::ENABLE_VIDEO;
+    config_.enable_audio = DevelopmentPreset::ENABLE_AUDIO;
+}
+
+void Config::loadSafetyPreset() {
+    loadDefaults();
+    config_.control_frequency = SafetyPreset::CONTROL_FREQUENCY;
+    config_.state_publish_frequency = SafetyPreset::STATE_PUBLISH_FREQUENCY;
+    config_.max_linear_velocity = SafetyPreset::MAX_LINEAR_VELOCITY;
+    config_.max_angular_velocity = SafetyPreset::MAX_ANGULAR_VELOCITY;
+    config_.emergency_stop_timeout = SafetyPreset::EMERGENCY_STOP_TIMEOUT;
+    config_.enable_video = SafetyPreset::ENABLE_VIDEO;
+    config_.enable_audio = SafetyPreset::ENABLE_AUDIO;
+}
+
+bool Config::loadConfig(const std::string& configFile) {
+    // If compile-time config is enabled, skip JSON loading
+    if (config::USE_COMPILE_TIME_CONFIG) {
+        loadDefaults();
+        return true;
+    }
+    try {
+        std::ifstream file(configFile);
+        if (!file.is_open()) {
+            LOG_WARN("Config file not found: " + configFile + ", using defaults");
+            setDefaults();
+            return false;
+        }
+        
+        nlohmann::json j;
+        file >> j;
+        
+        // Load network settings
+        if (j.contains("network")) {
+            auto& network = j["network"];
+            if (network.contains("interface")) {
+                config_.network_interface = network["interface"];
+            }
+        }
+        
+        // Load MQTT settings
+        if (j.contains("mqtt")) {
+            auto& mqtt = j["mqtt"];
+            if (mqtt.contains("broker")) config_.mqtt_broker = mqtt["broker"];
+            if (mqtt.contains("port")) config_.mqtt_port = mqtt["port"];
+            if (mqtt.contains("username")) config_.mqtt_username = mqtt["username"];
+            if (mqtt.contains("password")) config_.mqtt_password = mqtt["password"];
+            if (mqtt.contains("client_id")) config_.mqtt_client_id = mqtt["client_id"];
+        }
+        
+        // Load topics
+        if (j.contains("topics")) {
+            auto& topics = j["topics"];
+            if (topics.contains("prefix")) config_.topic_prefix = topics["prefix"];
+            if (topics.contains("cmd")) config_.cmd_topic = topics["cmd"];
+            if (topics.contains("state")) config_.state_topic = topics["state"];
+            if (topics.contains("video")) config_.video_topic = topics["video"];
+            if (topics.contains("audio")) config_.audio_topic = topics["audio"];
+        }
+        
+        // Load robot settings
+        if (j.contains("robot")) {
+            auto& robot = j["robot"];
+            if (robot.contains("control_frequency")) config_.control_frequency = robot["control_frequency"];
+            if (robot.contains("state_publish_frequency")) config_.state_publish_frequency = robot["state_publish_frequency"];
+            if (robot.contains("enable_video")) config_.enable_video = robot["enable_video"];
+            if (robot.contains("enable_audio")) config_.enable_audio = robot["enable_audio"];
+        }
+        
+        // Load safety settings
+        if (j.contains("safety")) {
+            auto& safety = j["safety"];
+            if (safety.contains("max_linear_velocity")) config_.max_linear_velocity = safety["max_linear_velocity"];
+            if (safety.contains("max_angular_velocity")) config_.max_angular_velocity = safety["max_angular_velocity"];
+            if (safety.contains("emergency_stop_timeout")) config_.emergency_stop_timeout = safety["emergency_stop_timeout"];
+        }
+        
+        // Load motion settings
+        if (j.contains("motion")) {
+            auto& motion = j["motion"];
+            if (motion.contains("stand_height")) config_.stand_height = motion["stand_height"];
+            if (motion.contains("default_gait")) config_.default_gait = motion["default_gait"];
+        }
+        
+        // Load video settings
+        if (j.contains("video")) {
+            auto& video = j["video"];
+            if (video.contains("width")) config_.video_width = video["width"];
+            if (video.contains("height")) config_.video_height = video["height"];
+            if (video.contains("fps")) config_.video_fps = video["fps"];
+            if (video.contains("format")) config_.video_format = video["format"];
+        }
+        
+        // Load audio settings
+        if (j.contains("audio")) {
+            auto& audio = j["audio"];
+            if (audio.contains("sample_rate")) config_.audio_sample_rate = audio["sample_rate"];
+            if (audio.contains("channels")) config_.audio_channels = audio["channels"];
+            if (audio.contains("format")) config_.audio_format = audio["format"];
+        }
+        
+        LOG_INFO("Configuration loaded successfully from: " + configFile);
+        return true;
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to load config: " + std::string(e.what()));
+        setDefaults();
+        return false;
+    }
+}
+
+bool Config::saveConfig(const std::string& configFile) {
+    try {
+        nlohmann::json j;
+        
+        // Network settings
+        j["network"]["interface"] = config_.network_interface;
+        
+        // MQTT settings
+        j["mqtt"]["broker"] = config_.mqtt_broker;
+        j["mqtt"]["port"] = config_.mqtt_port;
+        j["mqtt"]["username"] = config_.mqtt_username;
+        j["mqtt"]["password"] = config_.mqtt_password;
+        j["mqtt"]["client_id"] = config_.mqtt_client_id;
+        
+        // Topics
+        j["topics"]["prefix"] = config_.topic_prefix;
+        j["topics"]["cmd"] = config_.cmd_topic;
+        j["topics"]["state"] = config_.state_topic;
+        j["topics"]["video"] = config_.video_topic;
+        j["topics"]["audio"] = config_.audio_topic;
+        
+        // Robot settings
+        j["robot"]["control_frequency"] = config_.control_frequency;
+        j["robot"]["state_publish_frequency"] = config_.state_publish_frequency;
+        j["robot"]["enable_video"] = config_.enable_video;
+        j["robot"]["enable_audio"] = config_.enable_audio;
+        
+        // Safety settings
+        j["safety"]["max_linear_velocity"] = config_.max_linear_velocity;
+        j["safety"]["max_angular_velocity"] = config_.max_angular_velocity;
+        j["safety"]["emergency_stop_timeout"] = config_.emergency_stop_timeout;
+        
+        // Motion settings
+        j["motion"]["stand_height"] = config_.stand_height;
+        j["motion"]["default_gait"] = config_.default_gait;
+        
+        // Video settings
+        j["video"]["width"] = config_.video_width;
+        j["video"]["height"] = config_.video_height;
+        j["video"]["fps"] = config_.video_fps;
+        j["video"]["format"] = config_.video_format;
+        
+        // Audio settings
+        j["audio"]["sample_rate"] = config_.audio_sample_rate;
+        j["audio"]["channels"] = config_.audio_channels;
+        j["audio"]["format"] = config_.audio_format;
+        
+        std::ofstream file(configFile);
+        if (!file.is_open()) {
+            LOG_ERROR("Failed to open config file for writing: " + configFile);
+            return false;
+        }
+        
+        file << j.dump(4) << std::endl;
+        LOG_INFO("Configuration saved to: " + configFile);
+        return true;
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to save config: " + std::string(e.what()));
+        return false;
+    }
+}
+
+std::string Config::getFullTopic(const std::string& topic) const {
+    return config_.topic_prefix + "/" + topic;
+}
+
+bool Config::validateConfig() {
+    bool valid = true;
+    
+    // Validate frequencies
+    if (config_.control_frequency <= 0 || config_.control_frequency > 1000) {
+        LOG_WARN("Invalid control frequency, setting to default");
+        config_.control_frequency = config::CONTROL_FREQUENCY;
+        valid = false;
+    }
+    
+    if (config_.state_publish_frequency <= 0 || config_.state_publish_frequency > 500) {
+        LOG_WARN("Invalid state publish frequency, setting to default");
+        config_.state_publish_frequency = config::STATE_PUBLISH_FREQUENCY;
+        valid = false;
+    }
+    
+    // Validate velocities
+    if (config_.max_linear_velocity <= 0 || config_.max_linear_velocity > 5.0) {
+        LOG_WARN("Invalid max linear velocity, setting to default");
+        config_.max_linear_velocity = config::MAX_LINEAR_VELOCITY;
+        valid = false;
+    }
+    
+    if (config_.max_angular_velocity <= 0 || config_.max_angular_velocity > 10.0) {
+        LOG_WARN("Invalid max angular velocity, setting to default");
+        config_.max_angular_velocity = config::MAX_ANGULAR_VELOCITY;
+        valid = false;
+    }
+    
+    // Validate timeout
+    if (config_.emergency_stop_timeout <= 0 || config_.emergency_stop_timeout > 60.0) {
+        LOG_WARN("Invalid emergency stop timeout, setting to default");
+        config_.emergency_stop_timeout = config::EMERGENCY_STOP_TIMEOUT;
+        valid = false;
+    }
+    
+    // Validate video settings
+    if (config_.video_width <= 0 || config_.video_height <= 0) {
+        LOG_WARN("Invalid video dimensions, setting to default");
+        config_.video_width = config::VIDEO_WIDTH;
+        config_.video_height = config::VIDEO_HEIGHT;
+        valid = false;
+    }
+    
+    if (config_.video_fps <= 0 || config_.video_fps > 120) {
+        LOG_WARN("Invalid video FPS, setting to default");
+        config_.video_fps = config::VIDEO_FPS;
+        valid = false;
+    }
+    
+    // Validate audio settings
+    if (config_.audio_sample_rate <= 0) {
+        LOG_WARN("Invalid audio sample rate, setting to default");
+        config_.audio_sample_rate = config::AUDIO_SAMPLE_RATE;
+        valid = false;
+    }
+    
+    if (config_.audio_channels <= 0) {
+        LOG_WARN("Invalid audio channels, setting to default");
+        config_.audio_channels = config::AUDIO_CHANNELS;
+        valid = false;
+    }
+    
+    return valid;
+}
+
+} // namespace custom

src/controller.cpp

@@ -0,0 +1,575 @@
+#include "controller.hpp"
+#include "config.hpp"
+#include "logger.hpp"
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/common/thread/thread.hpp>
+
+namespace custom {
+
+Controller::Controller(const std::string& networkInterface)
+    : networkInterface_(networkInterface), running_(false), initialized_(false),
+      emergencyStopActive_(false) {
+    
+    auto& config = Config::getInstance().getConfig();
+    controlPeriod_ = std::chrono::milliseconds(static_cast<int>(1000.0 / config.control_frequency));
+    statePeriod_ = std::chrono::milliseconds(static_cast<int>(1000.0 / config.state_publish_frequency));
+}
+
+Controller::~Controller() {
+    shutdown();
+}
+
+bool Controller::initialize() {
+    try {
+        LOG_INFO("Initializing robot controller with interface: " + networkInterface_);
+        
+        // Initialize channel factory
+        unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface_);
+        
+        // Initialize sport client
+        sportClient_ = std::make_unique<unitree::robot::go2::SportClient>();
+        sportClient_->SetTimeout(10.0f);
+        sportClient_->Init();
+        
+        // Initialize obstacle avoidance client
+        obstacleClient_ = std::make_unique<unitree::robot::go2::ObstaclesAvoidClient>();
+        obstacleClient_->Init();
+        
+        // Initialize video client
+        videoClient_ = std::make_unique<unitree::robot::go2::VideoClient>();
+        videoClient_->Init();
+        
+        // Initialize VUI client
+        vuiClient_ = std::make_unique<unitree::robot::go2::VuiClient>();
+        vuiClient_->Init();
+        
+        // Initialize state subscribers
+        sportStateSubscriber_.reset(
+            new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>("rt/sportmodestate")
+        );
+        sportStateSubscriber_->InitChannel(
+            std::bind(&Controller::updateSportModeState, this, std::placeholders::_1), 1
+        );
+        
+        lowStateSubscriber_.reset(
+            new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>("rt/lowstate")
+        );
+        lowStateSubscriber_->InitChannel(
+            std::bind(&Controller::updateLowLevelState, this, std::placeholders::_1), 1
+        );
+        
+        // Initialize robot state
+        {
+            std::lock_guard<std::mutex> lock(stateMutex_);
+            currentState_ = RobotState{};
+            currentState_.timestamp = std::chrono::steady_clock::now();
+        }
+        
+        initialized_ = true;
+        LOG_INFO("Robot controller initialized successfully");
+        return true;
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to initialize robot controller: " + std::string(e.what()));
+        return false;
+    }
+}
+
+void Controller::shutdown() {
+    if (running_) {
+        stop();
+    }
+    
+    LOG_INFO("Shutting down robot controller");
+    
+    // Reset all clients
+    sportClient_.reset();
+    obstacleClient_.reset();
+    videoClient_.reset();
+    vuiClient_.reset();
+    
+    // Reset subscribers
+    sportStateSubscriber_.reset();
+    lowStateSubscriber_.reset();
+    
+    initialized_ = false;
+}
+
+bool Controller::start() {
+    if (!initialized_) {
+        LOG_ERROR("Cannot start: robot controller not initialized");
+        return false;
+    }
+    
+    if (running_) {
+        LOG_WARN("Robot controller already running");
+        return true;
+    }
+    
+    LOG_INFO("Starting robot controller");
+    
+    running_ = true;
+    emergencyStopActive_ = false;
+    lastCommandTime_ = std::chrono::steady_clock::now();
+    
+    // Start control threads
+    controlThread_ = std::thread(&Controller::controlLoop, this);
+    stateThread_ = std::thread(&Controller::stateUpdateLoop, this);
+    
+    LOG_INFO("Robot controller started successfully");
+    return true;
+}
+
+bool Controller::stop() {
+    if (!running_) {
+        return true;
+    }
+    
+    LOG_INFO("Stopping robot controller");
+    
+    // Stop all motion first
+    emergencyStop();
+    
+    running_ = false;
+    commandCondition_.notify_all();
+    
+    // Wait for threads to finish
+    if (controlThread_.joinable()) {
+        controlThread_.join();
+    }
+    if (stateThread_.joinable()) {
+        stateThread_.join();
+    }
+    
+    LOG_INFO("Robot controller stopped");
+    return true;
+}
+
+RobotState Controller::getCurrentState() const {
+    std::lock_guard<std::mutex> lock(stateMutex_);
+    return currentState_;
+}
+
+void Controller::setStateCallback(StateCallback callback) {
+    stateCallback_ = callback;
+}
+
+void Controller::setErrorCallback(ErrorCallback callback) {
+    errorCallback_ = callback;
+}
+
+bool Controller::executeCommand(const MotionCommand& command) {
+    if (!running_) {
+        LOG_ERROR("Cannot execute command: robot controller not running");
+        return false;
+    }
+    
+    if (emergencyStopActive_) {
+        LOG_WARN("Cannot execute command: emergency stop active");
+        return false;
+    }
+    
+    if (!validateCommand(command)) {
+        LOG_ERROR("Invalid motion command");
+        return false;
+    }
+    
+    {
+        std::lock_guard<std::mutex> lock(commandMutex_);
+        commandQueue_.push(command);
+        lastCommandTime_ = std::chrono::steady_clock::now();
+    }
+    commandCondition_.notify_one();
+    
+    return true;
+}
+
+bool Controller::emergencyStop() {
+    LOG_WARN("Emergency stop activated");
+    
+    emergencyStopActive_ = true;
+    
+    try {
+        if (sportClient_) {
+            sportClient_->StopMove();
+            sportClient_->Damp();
+        }
+        return true;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Emergency stop failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::recoveryStand() {
+    LOG_INFO("Executing recovery stand");
+    
+    try {
+        if (sportClient_) {
+            int32_t result = sportClient_->RecoveryStand();
+            if (result == 0) {
+                emergencyStopActive_ = false;
+                return true;
+            }
+        }
+        return false;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Recovery stand failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+// Basic motions
+bool Controller::standUp() {
+    LOG_INFO("Standing up");
+    try {
+        return sportClient_ && sportClient_->StandUp() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Stand up failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::standDown() {
+    LOG_INFO("Standing down");
+    try {
+        return sportClient_ && sportClient_->StandDown() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Stand down failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::sit() {
+    LOG_INFO("Sitting");
+    try {
+        return sportClient_ && sportClient_->Sit() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Sit failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::lie() {
+    LOG_INFO("Lying down");
+    try {
+        return sportClient_ && sportClient_->StandDown() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Lie down failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::damp() {
+    LOG_INFO("Damping");
+    try {
+        return sportClient_ && sportClient_->Damp() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Damp failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+// Movement
+bool Controller::move(double vx, double vy, double vyaw) {
+    try {
+        auto& config = Config::getInstance().getConfig();
+        
+        // Apply safety limits
+        vx = std::max(-config.max_linear_velocity, std::min(config.max_linear_velocity, vx));
+        vy = std::max(-config.max_linear_velocity, std::min(config.max_linear_velocity, vy));
+        vyaw = std::max(-config.max_angular_velocity, std::min(config.max_angular_velocity, vyaw));
+        
+        return sportClient_ && sportClient_->Move(vx, vy, vyaw) == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Move failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::moveToPosition(double x, double y, double yaw) {
+    try {
+        return obstacleClient_ && obstacleClient_->MoveToAbsolutePosition(x, y, yaw) == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Move to position failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::stop() {
+    try {
+        return sportClient_ && sportClient_->StopMove() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Stop failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+// Body control
+bool Controller::setBodyPose(double roll, double pitch, double yaw, double height) {
+    try {
+        if (sportClient_) {
+            sportClient_->Euler(roll, pitch, yaw);
+            sportClient_->BodyHeight(height);
+            return true;
+        }
+        return false;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Set body pose failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::balanceStand() {
+    try {
+        return sportClient_ && sportClient_->BalanceStand() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Balance stand failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+// Special actions
+bool Controller::dance1() {
+    try {
+        return sportClient_ && sportClient_->Dance1() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Dance1 failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::dance2() {
+    try {
+        return sportClient_ && sportClient_->Dance2() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Dance2 failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool Controller::hello() {
+    try {
+        return sportClient_ && sportClient_->Hello() == 0;
+    } catch (const std::exception& e) {
+        LOG_ERROR("Hello failed: " + std::string(e.what()));
+        return false;
+    }
+}
+
+// Control loops
+void Controller::controlLoop() {
+    LOG_INFO("Control loop started");
+    
+    while (running_) {
+        auto startTime = std::chrono::steady_clock::now();
+        
+        try {
+            processCommands();
+            checkSafetyLimits();
+        } catch (const std::exception& e) {
+            LOG_ERROR("Control loop error: " + std::string(e.what()));
+            handleError(e.what());
+        }
+        
+        auto endTime = std::chrono::steady_clock::now();
+        auto elapsed = endTime - startTime;
+        
+        if (elapsed < controlPeriod_) {
+            std::this_thread::sleep_for(controlPeriod_ - elapsed);
+        }
+    }
+    
+    LOG_INFO("Control loop stopped");
+}
+
+void Controller::stateUpdateLoop() {
+    LOG_INFO("State update loop started");
+    
+    while (running_) {
+        auto startTime = std::chrono::steady_clock::now();
+        
+        try {
+            // State is updated via callbacks, just trigger callback here
+            if (stateCallback_) {
+                RobotState state = getCurrentState();
+                stateCallback_(state);
+            }
+        } catch (const std::exception& e) {
+            LOG_ERROR("State update error: " + std::string(e.what()));
+        }
+        
+        auto endTime = std::chrono::steady_clock::now();
+        auto elapsed = endTime - startTime;
+        
+        if (elapsed < statePeriod_) {
+            std::this_thread::sleep_for(statePeriod_ - elapsed);
+        }
+    }
+    
+    LOG_INFO("State update loop stopped");
+}
+
+void Controller::processCommands() {
+    std::unique_lock<std::mutex> lock(commandMutex_);
+    
+    while (!commandQueue_.empty()) {
+        auto command = commandQueue_.front();
+        commandQueue_.pop();
+        lock.unlock();
+        
+        executeMotionCommand(command);
+        
+        lock.lock();
+    }
+}
+
+void Controller::executeMotionCommand(const MotionCommand& command) {
+    switch (command.type) {
+        case MotionCommand::VELOCITY:
+            move(command.linear_velocity[0], command.linear_velocity[1], command.angular_velocity[2]);
+            break;
+            
+        case MotionCommand::POSITION:
+            moveToPosition(command.target_position[0], command.target_position[1], command.target_orientation[2]);
+            break;
+            
+        case MotionCommand::BODY_POSE:
+            setBodyPose(command.body_roll, command.body_pitch, command.body_yaw, command.body_height);
+            break;
+            
+        case MotionCommand::SPECIAL_ACTION:
+            performAction(command.action_name, command.action_params);
+            break;
+            
+        default:
+            LOG_WARN("Unknown motion command type");
+            break;
+    }
+}
+
+bool Controller::validateCommand(const MotionCommand& command) {
+    auto& config = Config::getInstance().getConfig();
+    
+    // Check velocity limits
+    if (command.type == MotionCommand::VELOCITY) {
+        double vx = std::abs(command.linear_velocity[0]);
+        double vy = std::abs(command.linear_velocity[1]);
+        double vyaw = std::abs(command.angular_velocity[2]);
+        
+        if (vx > config.max_linear_velocity || vy > config.max_linear_velocity || 
+            vyaw > config.max_angular_velocity) {
+            return false;
+        }
+    }
+    
+    return true;
+}
+
+void Controller::updateSportModeState(const void* message) {
+    auto* state = static_cast<const unitree_go::msg::dds_::SportModeState_*>(message);
+    
+    std::lock_guard<std::mutex> lock(stateMutex_);
+    
+    // Update position
+    currentState_.position[0] = state->position()[0];
+    currentState_.position[1] = state->position()[1];
+    currentState_.position[2] = state->position()[2];
+    
+    // Update orientation
+    currentState_.orientation[0] = state->imu_state().rpy()[0];
+    currentState_.orientation[1] = state->imu_state().rpy()[1];
+    currentState_.orientation[2] = state->imu_state().rpy()[2];
+    
+    // Update velocity
+    currentState_.velocity[0] = state->velocity()[0];
+    currentState_.velocity[1] = state->velocity()[1];
+    currentState_.velocity[2] = state->velocity()[2];
+    
+    // Update IMU
+    for (int i = 0; i < 3; i++) {
+        currentState_.imu_acc[i] = state->imu_state().accelerometer()[i];
+        currentState_.imu_gyro[i] = state->imu_state().gyroscope()[i];
+    }
+    
+    for (int i = 0; i < 4; i++) {
+        currentState_.imu_quat[i] = state->imu_state().quaternion()[i];
+    }
+    
+    currentState_.timestamp = std::chrono::steady_clock::now();
+    currentState_.is_connected = true;
+}
+
+void Controller::updateLowLevelState(const void* message) {
+    auto* state = static_cast<const unitree_go::msg::dds_::LowState_*>(message);
+    
+    std::lock_guard<std::mutex> lock(stateMutex_);
+    
+    // Update motor states
+    for (int i = 0; i < 20 && i < state->motor_state().size(); i++) {
+        currentState_.motor_positions[i] = state->motor_state()[i].q();
+        currentState_.motor_velocities[i] = state->motor_state()[i].dq();
+        currentState_.motor_torques[i] = state->motor_state()[i].tau_est();
+        currentState_.motor_temperatures[i] = state->motor_state()[i].temperature();
+    }
+    
+    // Update battery info
+    if (!state->bms_state().empty()) {
+        currentState_.battery_voltage = state->bms_state()[0].voltage();
+        currentState_.battery_current = state->bms_state()[0].current();
+        currentState_.battery_percentage = state->bms_state()[0].soc();
+    }
+    
+    currentState_.timestamp = std::chrono::steady_clock::now();
+}
+
+void Controller::checkSafetyLimits() {
+    auto& config = Config::getInstance().getConfig();
+    auto now = std::chrono::steady_clock::now();
+    
+    // Check command timeout
+    auto timeSinceLastCommand = std::chrono::duration_cast<std::chrono::seconds>(
+        now - lastCommandTime_).count();
+    
+    if (timeSinceLastCommand > config.emergency_stop_timeout && !emergencyStopActive_) {
+        LOG_WARN("Command timeout exceeded, activating emergency stop");
+        emergencyStop();
+    }
+    
+    // Check battery level
+    {
+        std::lock_guard<std::mutex> lock(stateMutex_);
+        if (currentState_.battery_percentage < 10) {
+            currentState_.low_battery = true;
+            if (!emergencyStopActive_) {
+                LOG_WARN("Low battery detected, activating emergency stop");
+                emergencyStop();
+            }
+        }
+    }
+}
+
+void Controller::handleError(const std::string& error) {
+    if (errorCallback_) {
+        errorCallback_(error);
+    }
+}
+
+bool Controller::performAction(const std::string& actionName, const nlohmann::json& params) {
+    LOG_INFO("Performing action: " + actionName);
+    
+    if (actionName == "dance1") return dance1();
+    if (actionName == "dance2") return dance2();
+    if (actionName == "hello") return hello();
+    if (actionName == "sit") return sit();
+    if (actionName == "stand_up") return standUp();
+    if (actionName == "stand_down") return standDown();
+    if (actionName == "damp") return damp();
+    if (actionName == "balance_stand") return balanceStand();
+    if (actionName == "recovery_stand") return recoveryStand();
+    
+    LOG_WARN("Unknown action: " + actionName);
+    return false;
+}
+
+} // namespace custom

src/custom_robot.cpp

@@ -0,0 +1,682 @@
+#include "custom_robot.hpp"
+#include <chrono>
+#include <sstream>
+
+namespace custom {
+
+CustomRobot::CustomRobot(const std::string& configFile)
+    : configFile_(configFile.empty() ? "config/robot_config.json" : configFile),
+      running_(false), initialized_(false), mqttConnected_(false) {
+    
+    stats_.startTime = std::chrono::steady_clock::now();
+}
+
+CustomRobot::~CustomRobot() {
+    shutdown();
+}
+
+bool CustomRobot::initialize() {
+    LOG_INFO("Initializing CustomRobot");
+    
+    // Load configuration
+    if (!Config::getInstance().loadConfig(configFile_)) {
+        LOG_WARN("Failed to load config file, using defaults");
+    }
+    
+    auto& config = Config::getInstance().getConfig();
+    
+    // Initialize robot controller
+    robotController_ = std::make_unique<Controller>(config.network_interface);
+    if (!robotController_->initialize()) {
+        LOG_ERROR("Failed to initialize robot controller");
+        return false;
+    }
+    
+    // Set robot controller callbacks
+    robotController_->setStateCallback(
+        std::bind(&CustomRobot::handleRobotState, this, std::placeholders::_1)
+    );
+    robotController_->setErrorCallback(
+        std::bind(&CustomRobot::handleRobotError, this, std::placeholders::_1)
+    );
+    
+    // Initialize MQTT client
+    mqttClient_ = std::make_unique<MqttClient>(
+        config.mqtt_broker, config.mqtt_port, config.mqtt_client_id
+    );
+    
+    // Set MQTT callbacks
+    mqttClient_->setMessageCallback(
+        std::bind(&CustomRobot::handleMqttMessage, this, std::placeholders::_1, std::placeholders::_2)
+    );
+    mqttClient_->setConnectionCallback(
+        std::bind(&CustomRobot::handleMqttConnection, this, std::placeholders::_1)
+    );
+    
+    // Connect to MQTT broker
+    if (!mqttClient_->connect(config.mqtt_username, config.mqtt_password)) {
+        LOG_ERROR("Failed to connect to MQTT broker");
+        return false;
+    }
+    
+    // Start MQTT message processor
+    mqttClient_->startMessageProcessor();
+    
+    // Subscribe to command topics
+    std::string cmdTopic = Config::getInstance().getFullTopic(config.cmd_topic);
+    if (!mqttClient_->subscribe(cmdTopic + "/+")) { // Subscribe to all command subtopics
+        LOG_ERROR("Failed to subscribe to command topics");
+        return false;
+    }
+    
+    initialized_ = true;
+    LOG_INFO("CustomRobot initialized successfully");
+    return true;
+}
+
+void CustomRobot::shutdown() {
+    LOG_INFO("Shutting down CustomRobot");
+    
+    stop();
+    
+    if (mqttClient_) {
+        mqttClient_->stopMessageProcessor();
+        mqttClient_->disconnect();
+        mqttClient_.reset();
+    }
+    
+    if (robotController_) {
+        robotController_->shutdown();
+        robotController_.reset();
+    }
+    
+    initialized_ = false;
+}
+
+bool CustomRobot::start() {
+    if (!initialized_) {
+        LOG_ERROR("Cannot start: not initialized");
+        return false;
+    }
+    
+    if (running_) {
+        LOG_WARN("CustomRobot already running");
+        return true;
+    }
+    
+    LOG_INFO("Starting CustomRobot");
+    
+    // Start robot controller
+    if (!robotController_->start()) {
+        LOG_ERROR("Failed to start robot controller");
+        return false;
+    }
+    
+    running_ = true;
+    
+    // Start periodic tasks
+    startPeriodicTasks();
+    
+    // Publish initial status
+    publishHeartbeat();
+    
+    LOG_INFO("CustomRobot started successfully");
+    return true;
+}
+
+bool CustomRobot::stop() {
+    if (!running_) {
+        return true;
+    }
+    
+    LOG_INFO("Stopping CustomRobot");
+    
+    running_ = false;
+    
+    // Stop periodic tasks
+    stopPeriodicTasks();
+    
+    // Stop robot controller
+    if (robotController_) {
+        robotController_->stop();
+    }
+    
+    // Publish final status
+    if (mqttClient_ && mqttClient_->isConnected()) {
+        nlohmann::json status;
+        status["status"] = "stopped";
+        status["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
+            std::chrono::system_clock::now().time_since_epoch()).count();
+        
+        auto& config = Config::getInstance().getConfig();
+        std::string statusTopic = Config::getInstance().getFullTopic(config.state_topic + "/status");
+        mqttClient_->publishJson(statusTopic, status);
+    }
+    
+    LOG_INFO("CustomRobot stopped");
+    return true;
+}
+
+void CustomRobot::run() {
+    if (!start()) {
+        LOG_ERROR("Failed to start CustomRobot");
+        return;
+    }
+    
+    LOG_INFO("CustomRobot main loop started");
+    
+    while (running_) {
+        try {
+            // Process any pending errors
+            processErrorQueue();
+            
+            // Sleep for a short period
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            
+        } catch (const std::exception& e) {
+            LOG_ERROR("Main loop error: " + std::string(e.what()));
+        }
+    }
+    
+    LOG_INFO("CustomRobot main loop ended");
+}
+
+void CustomRobot::runAsync() {
+    mainThread_ = std::thread(&CustomRobot::run, this);
+}
+
+void CustomRobot::waitForShutdown() {
+    if (mainThread_.joinable()) {
+        mainThread_.join();
+    }
+}
+
+void CustomRobot::handleMqttMessage(const std::string& topic, const std::string& payload) {
+    try {
+        LOG_DEBUG("Received MQTT message on topic: " + topic);
+        
+        auto& config = Config::getInstance().getConfig();
+        std::string cmdPrefix = Config::getInstance().getFullTopic(config.cmd_topic);
+        
+        if (topic.find(cmdPrefix) == 0) {
+            // Parse JSON payload
+            nlohmann::json message = nlohmann::json::parse(payload);
+            
+            // Extract command type from topic
+            std::string cmdType = topic.substr(cmdPrefix.length() + 1);
+            
+            if (cmdType == "motion") {
+                handleCommandMessage(message);
+            } else if (cmdType == "config") {
+                handleConfigMessage(message);
+            } else if (cmdType == "control") {
+                handleControlMessage(message);
+            } else {
+                LOG_WARN("Unknown command type: " + cmdType);
+            }
+            
+            stats_.commandsReceived++;
+        }
+        
+    } catch (const nlohmann::json::parse_error& e) {
+        LOG_ERROR("JSON parse error: " + std::string(e.what()));
+        publishError("Invalid JSON in message: " + std::string(e.what()));
+    } catch (const std::exception& e) {
+        LOG_ERROR("Message handling error: " + std::string(e.what()));
+        publishError("Message handling error: " + std::string(e.what()));
+    }
+}
+
+void CustomRobot::handleCommandMessage(const nlohmann::json& command) {
+    std::string requestId = command.value("request_id", "");
+    
+    try {
+        if (!command.contains("type")) {
+            publishResponse(requestId, false, "Missing command type");
+            return;
+        }
+        
+        std::string cmdType = command["type"];
+        bool success = false;
+        
+        if (cmdType == "motion") {
+            success = processMotionCommand(command);
+        } else if (cmdType == "special_action") {
+            success = processSpecialAction(command);
+        } else if (cmdType == "system") {
+            success = processSystemCommand(command);
+        } else {
+            publishResponse(requestId, false, "Unknown command type: " + cmdType);
+            return;
+        }
+        
+        if (success) {
+            stats_.commandsExecuted++;
+            publishResponse(requestId, true);
+        } else {
+            stats_.commandsFailed++;
+            publishResponse(requestId, false, "Command execution failed");
+        }
+        
+        lastCommandReceived_ = std::chrono::steady_clock::now();
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Command processing error: " + std::string(e.what()));
+        publishResponse(requestId, false, "Command processing error: " + std::string(e.what()));
+        stats_.commandsFailed++;
+    }
+}
+
+bool CustomRobot::processMotionCommand(const nlohmann::json& cmd) {
+    if (!robotController_) {
+        return false;
+    }
+    
+    MotionCommand motionCmd = jsonToMotionCommand(cmd);
+    return robotController_->executeCommand(motionCmd);
+}
+
+bool CustomRobot::processSpecialAction(const nlohmann::json& cmd) {
+    if (!robotController_ || !cmd.contains("action")) {
+        return false;
+    }
+    
+    std::string action = cmd["action"];
+    nlohmann::json params = cmd.value("params", nlohmann::json::object());
+    
+    return robotController_->performAction(action, params);
+}
+
+bool CustomRobot::processSystemCommand(const nlohmann::json& cmd) {
+    if (!cmd.contains("command")) {
+        return false;
+    }
+    
+    std::string sysCmd = cmd["command"];
+    
+    if (sysCmd == "emergency_stop") {
+        return robotController_ && robotController_->emergencyStop();
+    } else if (sysCmd == "recovery_stand") {
+        return robotController_ && robotController_->recoveryStand();
+    } else if (sysCmd == "start") {
+        return robotController_ && robotController_->start();
+    } else if (sysCmd == "stop") {
+        return robotController_ && robotController_->stop();
+    }
+    
+    return false;
+}
+
+void CustomRobot::handleConfigMessage(const nlohmann::json& config) {
+    // Handle configuration updates
+    LOG_INFO("Received configuration update");
+    
+    try {
+        // Update configuration and save
+        auto& currentConfig = Config::getInstance().getConfig();
+        
+        if (config.contains("control_frequency")) {
+            currentConfig.control_frequency = config["control_frequency"];
+        }
+        if (config.contains("state_publish_frequency")) {
+            currentConfig.state_publish_frequency = config["state_publish_frequency"];
+        }
+        
+        // Save updated configuration
+        Config::getInstance().saveConfig(configFile_);
+        
+        LOG_INFO("Configuration updated successfully");
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Configuration update error: " + std::string(e.what()));
+    }
+}
+
+void CustomRobot::handleControlMessage(const nlohmann::json& control) {
+    // Handle control messages (start/stop/pause etc.)
+    if (control.contains("command")) {
+        std::string cmd = control["command"];
+        
+        if (cmd == "start" && !running_) {
+            start();
+        } else if (cmd == "stop" && running_) {
+            stop();
+        } else if (cmd == "restart") {
+            stop();
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+            start();
+        }
+    }
+}
+
+void CustomRobot::handleRobotState(const RobotState& state) {
+    // Cache the latest state
+    {
+        std::lock_guard<std::mutex> lock(stateCacheMutex_);
+        lastKnownState_ = state;
+    }
+    
+    // Publish state if enough time has passed
+    auto now = std::chrono::steady_clock::now();
+    auto timeSinceLastPublish = std::chrono::duration<double>(now - lastStatePublish_).count();
+    
+    if (timeSinceLastPublish >= statePublishInterval_) {
+        publishState();
+        lastStatePublish_ = now;
+    }
+}
+
+void CustomRobot::handleRobotError(const std::string& error) {
+    {
+        std::lock_guard<std::mutex> lock(errorMutex_);
+        errorQueue_.push(error);
+    }
+    
+    stats_.errorsOccurred++;
+}
+
+void CustomRobot::publishState() {
+    if (!mqttClient_ || !mqttClient_->isConnected()) {
+        return;
+    }
+    
+    try {
+        RobotState state;
+        {
+            std::lock_guard<std::mutex> lock(stateCacheMutex_);
+            state = lastKnownState_;
+        }
+        
+        nlohmann::json stateJson = robotStateToJson(state);
+        
+        auto& config = Config::getInstance().getConfig();
+        std::string stateTopic = Config::getInstance().getFullTopic(config.state_topic + "/robot");
+        
+        mqttClient_->publishJson(stateTopic, stateJson);
+        stats_.statesPublished++;
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("State publishing error: " + std::string(e.what()));
+    }
+}
+
+void CustomRobot::publishHeartbeat() {
+    if (!mqttClient_ || !mqttClient_->isConnected()) {
+        return;
+    }
+    
+    try {
+        nlohmann::json heartbeat = createStatusMessage();
+        
+        auto& config = Config::getInstance().getConfig();
+        std::string heartbeatTopic = Config::getInstance().getFullTopic(config.state_topic + "/heartbeat");
+        
+        mqttClient_->publishJson(heartbeatTopic, heartbeat);
+        lastHeartbeat_ = std::chrono::steady_clock::now();
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Heartbeat publishing error: " + std::string(e.what()));
+    }
+}
+
+void CustomRobot::publishError(const std::string& error) {
+    if (!mqttClient_ || !mqttClient_->isConnected()) {
+        return;
+    }
+    
+    try {
+        nlohmann::json errorMsg;
+        errorMsg["error"] = error;
+        errorMsg["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
+            std::chrono::system_clock::now().time_since_epoch()).count();
+        
+        auto& config = Config::getInstance().getConfig();
+        std::string errorTopic = Config::getInstance().getFullTopic(config.state_topic + "/error");
+        
+        mqttClient_->publishJson(errorTopic, errorMsg);
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error publishing error: " + std::string(e.what()));
+    }
+}
+
+void CustomRobot::publishResponse(const std::string& requestId, bool success, const std::string& message) {
+    if (!mqttClient_ || !mqttClient_->isConnected() || requestId.empty()) {
+        return;
+    }
+    
+    try {
+        nlohmann::json response;
+        response["request_id"] = requestId;
+        response["success"] = success;
+        response["message"] = message;
+        response["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
+            std::chrono::system_clock::now().time_since_epoch()).count();
+        
+        auto& config = Config::getInstance().getConfig();
+        std::string responseTopic = Config::getInstance().getFullTopic(config.state_topic + "/response");
+        
+        mqttClient_->publishJson(responseTopic, response);
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Response publishing error: " + std::string(e.what()));
+    }
+}
+
+MotionCommand CustomRobot::jsonToMotionCommand(const nlohmann::json& json) {
+    MotionCommand cmd;
+    cmd.timestamp = std::chrono::steady_clock::now();
+    
+    if (json.contains("motion_type")) {
+        std::string motionType = json["motion_type"];
+        if (motionType == "velocity") {
+            cmd.type = MotionCommand::VELOCITY;
+        } else if (motionType == "position") {
+            cmd.type = MotionCommand::POSITION;
+        } else if (motionType == "body_pose") {
+            cmd.type = MotionCommand::BODY_POSE;
+        } else if (motionType == "special_action") {
+            cmd.type = MotionCommand::SPECIAL_ACTION;
+        }
+    }
+    
+    // Parse velocity command
+    if (json.contains("linear_velocity")) {
+        auto vel = json["linear_velocity"];
+        if (vel.is_array() && vel.size() >= 3) {
+            cmd.linear_velocity[0] = vel[0];
+            cmd.linear_velocity[1] = vel[1];
+            cmd.linear_velocity[2] = vel[2];
+        }
+    }
+    
+    if (json.contains("angular_velocity")) {
+        auto vel = json["angular_velocity"];
+        if (vel.is_array() && vel.size() >= 3) {
+            cmd.angular_velocity[0] = vel[0];
+            cmd.angular_velocity[1] = vel[1];
+            cmd.angular_velocity[2] = vel[2];
+        }
+    }
+    
+    // Parse position command
+    if (json.contains("target_position")) {
+        auto pos = json["target_position"];
+        if (pos.is_array() && pos.size() >= 3) {
+            cmd.target_position[0] = pos[0];
+            cmd.target_position[1] = pos[1];
+            cmd.target_position[2] = pos[2];
+        }
+    }
+    
+    // Parse body pose
+    if (json.contains("body_height")) cmd.body_height = json["body_height"];
+    if (json.contains("body_roll")) cmd.body_roll = json["body_roll"];
+    if (json.contains("body_pitch")) cmd.body_pitch = json["body_pitch"];
+    if (json.contains("body_yaw")) cmd.body_yaw = json["body_yaw"];
+    
+    // Parse special action
+    if (json.contains("action_name")) {
+        cmd.action_name = json["action_name"];
+    }
+    if (json.contains("action_params")) {
+        cmd.action_params = json["action_params"];
+    }
+    
+    // Parse duration
+    if (json.contains("duration")) {
+        cmd.duration = json["duration"];
+    }
+    
+    return cmd;
+}
+
+nlohmann::json CustomRobot::robotStateToJson(const RobotState& state) {
+    nlohmann::json json;
+    
+    // Basic info
+    json["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
+        state.timestamp.time_since_epoch()).count();
+    json["is_connected"] = state.is_connected;
+    json["emergency_stop"] = state.emergency_stop;
+    
+    // Position and orientation
+    json["position"] = {state.position[0], state.position[1], state.position[2]};
+    json["orientation"] = {state.orientation[0], state.orientation[1], state.orientation[2]};
+    json["velocity"] = {state.velocity[0], state.velocity[1], state.velocity[2]};
+    
+    // IMU data
+    json["imu"]["acceleration"] = {state.imu_acc[0], state.imu_acc[1], state.imu_acc[2]};
+    json["imu"]["gyroscope"] = {state.imu_gyro[0], state.imu_gyro[1], state.imu_gyro[2]};
+    json["imu"]["quaternion"] = {state.imu_quat[0], state.imu_quat[1], state.imu_quat[2], state.imu_quat[3]};
+    
+    // Battery info
+    json["battery"]["voltage"] = state.battery_voltage;
+    json["battery"]["current"] = state.battery_current;
+    json["battery"]["percentage"] = state.battery_percentage;
+    json["battery"]["low_battery"] = state.low_battery;
+    
+    // System info
+    json["temperature"] = state.temperature;
+    json["overheated"] = state.overheated;
+    
+    // Motor states (first 12 leg motors)
+    json["motors"]["positions"] = std::vector<double>(state.motor_positions, state.motor_positions + 12);
+    json["motors"]["velocities"] = std::vector<double>(state.motor_velocities, state.motor_velocities + 12);
+    json["motors"]["torques"] = std::vector<double>(state.motor_torques, state.motor_torques + 12);
+    json["motors"]["temperatures"] = std::vector<double>(state.motor_temperatures, state.motor_temperatures + 12);
+    
+    return json;
+}
+
+nlohmann::json CustomRobot::createStatusMessage() {
+    nlohmann::json status;
+    
+    status["status"] = running_ ? "running" : "stopped";
+    status["initialized"] = initialized_;
+    status["mqtt_connected"] = mqttConnected_;
+    status["robot_connected"] = robotController_ && robotController_->isRunning();
+    
+    // Statistics
+    status["stats"]["commands_received"] = stats_.commandsReceived;
+    status["stats"]["commands_executed"] = stats_.commandsExecuted;
+    status["stats"]["commands_failed"] = stats_.commandsFailed;
+    status["stats"]["states_published"] = stats_.statesPublished;
+    status["stats"]["errors_occurred"] = stats_.errorsOccurred;
+    
+    // Uptime
+    auto now = std::chrono::steady_clock::now();
+    auto uptime = std::chrono::duration_cast<std::chrono::seconds>(now - stats_.startTime).count();
+    status["uptime_seconds"] = uptime;
+    
+    status["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    
+    return status;
+}
+
+void CustomRobot::startPeriodicTasks() {
+    // Heartbeat thread
+    periodicThreads_.emplace_back([this]() {
+        while (running_) {
+            auto now = std::chrono::steady_clock::now();
+            auto timeSinceLastHeartbeat = std::chrono::duration<double>(now - lastHeartbeat_).count();
+            
+            if (timeSinceLastHeartbeat >= heartbeatInterval_) {
+                publishHeartbeat();
+            }
+            
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+    });
+    
+    // Safety monitor thread
+    safetyThread_ = std::thread(&CustomRobot::safetyMonitorLoop, this);
+}
+
+void CustomRobot::stopPeriodicTasks() {
+    // Wait for all periodic threads to finish
+    for (auto& thread : periodicThreads_) {
+        if (thread.joinable()) {
+            thread.join();
+        }
+    }
+    periodicThreads_.clear();
+    
+    if (safetyThread_.joinable()) {
+        safetyThread_.join();
+    }
+}
+
+void CustomRobot::handleMqttConnection(bool connected) {
+    mqttConnected_ = connected;
+    
+    if (connected) {
+        LOG_INFO("MQTT connection established");
+        
+        // Resubscribe to topics
+        auto& config = Config::getInstance().getConfig();
+        std::string cmdTopic = Config::getInstance().getFullTopic(config.cmd_topic);
+        mqttClient_->subscribe(cmdTopic + "/+");
+        
+        // Publish connection status
+        publishHeartbeat();
+    } else {
+        LOG_WARN("MQTT connection lost");
+    }
+}
+
+void CustomRobot::safetyMonitorLoop() {
+    while (running_) {
+        try {
+            checkEmergencyConditions();
+            std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        } catch (const std::exception& e) {
+            LOG_ERROR("Safety monitor error: " + std::string(e.what()));
+        }
+    }
+}
+
+void CustomRobot::checkEmergencyConditions() {
+    // Check command timeout
+    auto now = std::chrono::steady_clock::now();
+    auto timeSinceLastCommand = std::chrono::duration<double>(now - lastCommandReceived_).count();
+    
+    if (timeSinceLastCommand > maxIdleTime_ && robotController_ && robotController_->isRunning()) {
+        LOG_WARN("Maximum idle time exceeded, stopping robot");
+        robotController_->emergencyStop();
+    }
+}
+
+void CustomRobot::processErrorQueue() {
+    std::lock_guard<std::mutex> lock(errorMutex_);
+    
+    while (!errorQueue_.empty()) {
+        std::string error = errorQueue_.front();
+        errorQueue_.pop();
+        
+        publishError(error);
+    }
+}
+
+} // namespace custom

src/logger.cpp

@@ -0,0 +1,87 @@
+#include "logger.hpp"
+#include <iostream>
+
+namespace custom {
+
+Logger& Logger::getInstance() {
+    static Logger instance;
+    return instance;
+}
+
+void Logger::setLevel(LogLevel level) {
+    std::lock_guard<std::mutex> lock(logMutex_);
+    currentLevel_ = level;
+}
+
+void Logger::setLogFile(const std::string& filename) {
+    std::lock_guard<std::mutex> lock(logMutex_);
+    if (logFile_) {
+        logFile_->close();
+    }
+    logFile_ = std::make_unique<std::ofstream>(filename, std::ios::app);
+    if (!logFile_->is_open()) {
+        std::cerr << "Failed to open log file: " << filename << std::endl;
+        logFile_.reset();
+    }
+}
+
+void Logger::log(LogLevel level, const std::string& message) {
+    if (level < currentLevel_) {
+        return;
+    }
+    
+    std::lock_guard<std::mutex> lock(logMutex_);
+    std::string timestamp = getCurrentTime();
+    std::string levelStr = levelToString(level);
+    
+    std::string logMessage = "[" + timestamp + "] [" + levelStr + "] " + message;
+    
+    // Always log to console
+    std::cout << logMessage << std::endl;
+    
+    // Log to file if available
+    if (logFile_ && logFile_->is_open()) {
+        *logFile_ << logMessage << std::endl;
+        logFile_->flush();
+    }
+}
+
+void Logger::debug(const std::string& message) {
+    log(LogLevel::DEBUG, message);
+}
+
+void Logger::info(const std::string& message) {
+    log(LogLevel::INFO, message);
+}
+
+void Logger::warn(const std::string& message) {
+    log(LogLevel::WARN, message);
+}
+
+void Logger::error(const std::string& message) {
+    log(LogLevel::ERROR, message);
+}
+
+std::string Logger::getCurrentTime() {
+    auto now = std::chrono::system_clock::now();
+    auto time_t = std::chrono::system_clock::to_time_t(now);
+    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+        now.time_since_epoch()) % 1000;
+    
+    std::stringstream ss;
+    ss << std::put_time(std::localtime(&time_t), "%Y-%m-%d %H:%M:%S");
+    ss << "." << std::setfill('0') << std::setw(3) << ms.count();
+    return ss.str();
+}
+
+std::string Logger::levelToString(LogLevel level) {
+    switch (level) {
+        case LogLevel::DEBUG: return "DEBUG";
+        case LogLevel::INFO:  return "INFO ";
+        case LogLevel::WARN:  return "WARN ";
+        case LogLevel::ERROR: return "ERROR";
+        default: return "UNKNOWN";
+    }
+}
+
+} // namespace custom

src/main.cpp

@@ -0,0 +1,54 @@
+#include <iostream>
+#include <memory>
+#include <signal.h>
+#include "custom_robot.hpp"
+#include "logger.hpp"
+#include "config.hpp"
+
+using namespace custom;
+
+std::unique_ptr<CustomRobot> g_robot;
+
+void signalHandler(int signal) {
+    LOG_INFO("Received signal " + std::to_string(signal) + ", shutting down...");
+    if (g_robot) {
+        g_robot->stop();
+    }
+    exit(0);
+}
+
+int main(int argc, char** argv) {
+    // Initialize logger with default settings
+    Logger::getInstance().setLevel(LogLevel::INFO);
+    
+    LOG_INFO("Starting Unitree GO2 System v0.0.1");
+    
+    // Setup signal handlers
+    signal(SIGINT, signalHandler);
+    signal(SIGTERM, signalHandler);
+    
+    try {
+        // Create robot with compile-time defaults (no config file needed)
+        g_robot = std::make_unique<CustomRobot>("");
+        
+        if (!g_robot->initialize()) {
+            LOG_ERROR("Failed to initialize robot");
+            return 1;
+        }
+        
+        LOG_INFO("Robot initialized successfully");
+        LOG_INFO("Press Ctrl+C to stop the robot");
+        
+        // Run the robot
+        g_robot->run();
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Exception: " + std::string(e.what()));
+        return 1;
+    } catch (...) {
+        LOG_ERROR("Unknown exception occurred");
+        return 1;
+    }
+    
+    return 0;
+}

src/mqtt.cpp

@@ -0,0 +1,265 @@
+#include "mqtt.hpp"
+#include "logger.hpp"
+#include <sstream>
+
+namespace custom {
+
+MqttClient::MqttClient(const std::string& broker, int port, const std::string& clientId)
+    : broker_(broker), port_(port), clientId_(clientId), 
+      connected_(false), reconnecting_(false), processorRunning_(false),
+      shouldReconnect_(true) {
+    
+    std::stringstream ss;
+    ss << "tcp://" << broker_ << ":" << port_;
+    std::string serverURI = ss.str();
+    
+    client_ = std::make_unique<mqtt::async_client>(serverURI, clientId_);
+    callback_ = std::make_unique<Callback>(this);
+    client_->set_callback(*callback_);
+}
+
+MqttClient::~MqttClient() {
+    shouldReconnect_ = false;
+    stopMessageProcessor();
+    disconnect();
+    
+    if (reconnectThread_.joinable()) {
+        reconnectThread_.join();
+    }
+}
+
+bool MqttClient::connect(const std::string& username, const std::string& password) {
+    try {
+        username_ = username;
+        password_ = password;
+        
+        mqtt::connect_options connOpts;
+        connOpts.set_keep_alive_interval(20);
+        connOpts.set_clean_session(true);
+        connOpts.set_automatic_reconnect(true);
+        
+        if (!username_.empty()) {
+            connOpts.set_user_name(username_);
+            if (!password_.empty()) {
+                connOpts.set_password(password_);
+            }
+        }
+        
+        LOG_INFO("Connecting to MQTT broker: " + broker_ + ":" + std::to_string(port_));
+        
+        auto token = client_->connect(connOpts, nullptr, *callback_);
+        token->wait_for(std::chrono::seconds(10));
+        
+        if (token->is_complete() && client_->is_connected()) {
+            connected_ = true;
+            LOG_INFO("Connected to MQTT broker successfully");
+            return true;
+        } else {
+            LOG_ERROR("Failed to connect to MQTT broker");
+            return false;
+        }
+        
+    } catch (const mqtt::exception& e) {
+        LOG_ERROR("MQTT connection exception: " + std::string(e.what()));
+        return false;
+    }
+}
+
+void MqttClient::disconnect() {
+    try {
+        if (client_ && client_->is_connected()) {
+            LOG_INFO("Disconnecting from MQTT broker");
+            auto token = client_->disconnect();
+            token->wait_for(std::chrono::seconds(5));
+            connected_ = false;
+        }
+    } catch (const mqtt::exception& e) {
+        LOG_ERROR("MQTT disconnect exception: " + std::string(e.what()));
+    }
+}
+
+bool MqttClient::isConnected() const {
+    return connected_ && client_ && client_->is_connected();
+}
+
+bool MqttClient::subscribe(const std::string& topic, int qos) {
+    try {
+        if (!isConnected()) {
+            LOG_ERROR("Cannot subscribe: not connected to MQTT broker");
+            return false;
+        }
+        
+        LOG_INFO("Subscribing to topic: " + topic);
+        auto token = client_->subscribe(topic, qos, nullptr, *callback_);
+        token->wait_for(std::chrono::seconds(5));
+        
+        return token->is_complete();
+        
+    } catch (const mqtt::exception& e) {
+        LOG_ERROR("MQTT subscribe exception: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool MqttClient::unsubscribe(const std::string& topic) {
+    try {
+        if (!isConnected()) {
+            LOG_ERROR("Cannot unsubscribe: not connected to MQTT broker");
+            return false;
+        }
+        
+        LOG_INFO("Unsubscribing from topic: " + topic);
+        auto token = client_->unsubscribe(topic, nullptr, *callback_);
+        token->wait_for(std::chrono::seconds(5));
+        
+        return token->is_complete();
+        
+    } catch (const mqtt::exception& e) {
+        LOG_ERROR("MQTT unsubscribe exception: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool MqttClient::publish(const std::string& topic, const std::string& payload, int qos, bool retain) {
+    try {
+        if (!isConnected()) {
+            LOG_ERROR("Cannot publish: not connected to MQTT broker");
+            return false;
+        }
+        
+        auto msg = mqtt::make_message(topic, payload);
+        msg->set_qos(qos);
+        msg->set_retained(retain);
+        
+        auto token = client_->publish(msg, nullptr, *callback_);
+        // Don't wait for completion to avoid blocking
+        
+        return true;
+        
+    } catch (const mqtt::exception& e) {
+        LOG_ERROR("MQTT publish exception: " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool MqttClient::publishJson(const std::string& topic, const nlohmann::json& json, int qos, bool retain) {
+    return publish(topic, json.dump(), qos, retain);
+}
+
+void MqttClient::setMessageCallback(MessageCallback callback) {
+    messageCallback_ = callback;
+}
+
+void MqttClient::setConnectionCallback(ConnectionCallback callback) {
+    connectionCallback_ = callback;
+}
+
+void MqttClient::startMessageProcessor() {
+    if (processorRunning_) {
+        return;
+    }
+    
+    processorRunning_ = true;
+    messageProcessor_ = std::thread(&MqttClient::processMessageQueue, this);
+    LOG_INFO("MQTT message processor started");
+}
+
+void MqttClient::stopMessageProcessor() {
+    if (!processorRunning_) {
+        return;
+    }
+    
+    processorRunning_ = false;
+    queueCondition_.notify_all();
+    
+    if (messageProcessor_.joinable()) {
+        messageProcessor_.join();
+    }
+    
+    LOG_INFO("MQTT message processor stopped");
+}
+
+void MqttClient::processMessageQueue() {
+    while (processorRunning_) {
+        std::unique_lock<std::mutex> lock(queueMutex_);
+        queueCondition_.wait(lock, [this] { 
+            return !messageQueue_.empty() || !processorRunning_; 
+        });
+        
+        while (!messageQueue_.empty() && processorRunning_) {
+            auto message = messageQueue_.front();
+            messageQueue_.pop();
+            lock.unlock();
+            
+            if (messageCallback_) {
+                try {
+                    messageCallback_(message.topic, message.payload);
+                } catch (const std::exception& e) {
+                    LOG_ERROR("Exception in message callback: " + std::string(e.what()));
+                }
+            }
+            
+            lock.lock();
+        }
+    }
+}
+
+void MqttClient::handleConnectionLost() {
+    connected_ = false;
+    LOG_WARN("MQTT connection lost");
+    
+    if (connectionCallback_) {
+        connectionCallback_(false);
+    }
+    
+    if (shouldReconnect_ && !reconnecting_) {
+        reconnecting_ = true;
+        reconnectThread_ = std::thread(&MqttClient::handleConnectionLost, this);
+    }
+}
+
+void MqttClient::handleConnectionEstablished() {
+    connected_ = true;
+    reconnecting_ = false;
+    LOG_INFO("MQTT connection established");
+    
+    if (connectionCallback_) {
+        connectionCallback_(true);
+    }
+}
+
+// Callback implementation
+void MqttClient::Callback::connected(const std::string& cause) {
+    client_->handleConnectionEstablished();
+}
+
+void MqttClient::Callback::connection_lost(const std::string& cause) {
+    LOG_WARN("MQTT connection lost: " + cause);
+    client_->handleConnectionLost();
+}
+
+void MqttClient::Callback::message_arrived(mqtt::const_message_ptr msg) {
+    QueuedMessage queuedMsg;
+    queuedMsg.topic = msg->get_topic();
+    queuedMsg.payload = msg->to_string();
+    
+    {
+        std::lock_guard<std::mutex> lock(client_->queueMutex_);
+        client_->messageQueue_.push(queuedMsg);
+    }
+    client_->queueCondition_.notify_one();
+}
+
+void MqttClient::Callback::delivery_complete(mqtt::delivery_token_ptr token) {
+    // Message delivered successfully
+}
+
+void MqttClient::Callback::on_failure(const mqtt::token& tok) {
+    LOG_ERROR("MQTT operation failed");
+}
+
+void MqttClient::Callback::on_success(const mqtt::token& tok) {
+    // Operation completed successfully
+}
+
+} // namespace custom