diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..53a5da2 --- /dev/null +++ b/.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.* diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..83b17e6 --- /dev/null +++ b/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} +) diff --git a/README.md b/README.md index bcfd9b9..8e9be5c 100644 --- a/README.md +++ b/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 \ No newline at end of file diff --git a/include/config.hpp b/include/config.hpp new file mode 100644 index 0000000..3f2f6d3 --- /dev/null +++ b/include/config.hpp @@ -0,0 +1,192 @@ +#pragma once + +#include +#include + +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 diff --git a/include/controller.hpp b/include/controller.hpp new file mode 100644 index 0000000..36c4711 --- /dev/null +++ b/include/controller.hpp @@ -0,0 +1,233 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +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; + using ErrorCallback = std::function; + + 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 sportClient_; + std::unique_ptr obstacleClient_; + std::unique_ptr videoClient_; + std::unique_ptr vuiClient_; + + // State subscribers + std::unique_ptr> sportStateSubscriber_; + std::unique_ptr> lowStateSubscriber_; + + // Current state + mutable std::mutex stateMutex_; + RobotState currentState_; + + // Command queue + std::queue commandQueue_; + std::mutex commandMutex_; + std::condition_variable commandCondition_; + + // Control threads + std::thread controlThread_; + std::thread stateThread_; + std::atomic running_; + std::atomic initialized_; + + // Callbacks + StateCallback stateCallback_; + ErrorCallback errorCallback_; + + // Safety + std::chrono::steady_clock::time_point lastCommandTime_; + std::atomic emergencyStopActive_; + + // Network + std::string networkInterface_; + + // Timing + std::chrono::milliseconds controlPeriod_; + std::chrono::milliseconds statePeriod_; +}; + +} // namespace custom diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp new file mode 100644 index 0000000..959f257 --- /dev/null +++ b/include/custom_robot.hpp @@ -0,0 +1,123 @@ +#pragma once + +#include "controller.hpp" +#include "mqtt.hpp" +#include "config.hpp" +#include "logger.hpp" + +#include +#include +#include +#include +#include + +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 robotController_; + std::unique_ptr mqttClient_; + + // Configuration + std::string configFile_; + + // State + std::atomic running_; + std::atomic initialized_; + std::atomic mqttConnected_; + + // Threads + std::thread mainThread_; + std::thread safetyThread_; + std::vector 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 pendingRequests_; + + // Error handling + std::mutex errorMutex_; + std::queue errorQueue_; + void processErrorQueue(); +}; + +} // namespace custom diff --git a/include/logger.hpp b/include/logger.hpp new file mode 100644 index 0000000..33fb3da --- /dev/null +++ b/include/logger.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +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 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 diff --git a/include/mqtt.hpp b/include/mqtt.hpp new file mode 100644 index 0000000..ffc4e4d --- /dev/null +++ b/include/mqtt.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace custom { + +class MqttClient { +public: + using MessageCallback = std::function; + using ConnectionCallback = std::function; + + 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 client_; + std::unique_ptr callback_; + + MessageCallback messageCallback_; + ConnectionCallback connectionCallback_; + + std::atomic connected_; + std::atomic reconnecting_; + + // Message processing + std::queue messageQueue_; + std::mutex queueMutex_; + std::condition_variable queueCondition_; + std::thread messageProcessor_; + std::atomic processorRunning_; + + // Connection parameters + std::string broker_; + int port_; + std::string clientId_; + std::string username_; + std::string password_; + + // Reconnection + std::thread reconnectThread_; + std::atomic shouldReconnect_; + int reconnectDelay_ = 5; // seconds +}; + +} // namespace custom diff --git a/scripts/install_deps.sh b/scripts/install_deps.sh new file mode 100644 index 0000000..e529057 --- /dev/null +++ b/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)" diff --git a/scripts/run_robot.sh b/scripts/run_robot.sh new file mode 100644 index 0000000..4430d1c --- /dev/null +++ b/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 diff --git a/src/config.cpp b/src/config.cpp new file mode 100644 index 0000000..42f1209 --- /dev/null +++ b/src/config.cpp @@ -0,0 +1,316 @@ +#include "config.hpp" +#include "logger.hpp" +#include +#include + +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 diff --git a/src/controller.cpp b/src/controller.cpp new file mode 100644 index 0000000..6fbea62 --- /dev/null +++ b/src/controller.cpp @@ -0,0 +1,575 @@ +#include "controller.hpp" +#include "config.hpp" +#include "logger.hpp" +#include +#include + +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(1000.0 / config.control_frequency)); + statePeriod_ = std::chrono::milliseconds(static_cast(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(); + sportClient_->SetTimeout(10.0f); + sportClient_->Init(); + + // Initialize obstacle avoidance client + obstacleClient_ = std::make_unique(); + obstacleClient_->Init(); + + // Initialize video client + videoClient_ = std::make_unique(); + videoClient_->Init(); + + // Initialize VUI client + vuiClient_ = std::make_unique(); + vuiClient_->Init(); + + // Initialize state subscribers + sportStateSubscriber_.reset( + new unitree::robot::ChannelSubscriber("rt/sportmodestate") + ); + sportStateSubscriber_->InitChannel( + std::bind(&Controller::updateSportModeState, this, std::placeholders::_1), 1 + ); + + lowStateSubscriber_.reset( + new unitree::robot::ChannelSubscriber("rt/lowstate") + ); + lowStateSubscriber_->InitChannel( + std::bind(&Controller::updateLowLevelState, this, std::placeholders::_1), 1 + ); + + // Initialize robot state + { + std::lock_guard 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 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 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 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(message); + + std::lock_guard 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(message); + + std::lock_guard 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( + 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 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 diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp new file mode 100644 index 0000000..cb0c940 --- /dev/null +++ b/src/custom_robot.cpp @@ -0,0 +1,682 @@ +#include "custom_robot.hpp" +#include +#include + +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(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( + 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::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 lock(stateCacheMutex_); + lastKnownState_ = state; + } + + // Publish state if enough time has passed + auto now = std::chrono::steady_clock::now(); + auto timeSinceLastPublish = std::chrono::duration(now - lastStatePublish_).count(); + + if (timeSinceLastPublish >= statePublishInterval_) { + publishState(); + lastStatePublish_ = now; + } +} + +void CustomRobot::handleRobotError(const std::string& error) { + { + std::lock_guard lock(errorMutex_); + errorQueue_.push(error); + } + + stats_.errorsOccurred++; +} + +void CustomRobot::publishState() { + if (!mqttClient_ || !mqttClient_->isConnected()) { + return; + } + + try { + RobotState state; + { + std::lock_guard 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::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::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( + 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(state.motor_positions, state.motor_positions + 12); + json["motors"]["velocities"] = std::vector(state.motor_velocities, state.motor_velocities + 12); + json["motors"]["torques"] = std::vector(state.motor_torques, state.motor_torques + 12); + json["motors"]["temperatures"] = std::vector(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(now - stats_.startTime).count(); + status["uptime_seconds"] = uptime; + + status["timestamp"] = std::chrono::duration_cast( + 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(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(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 lock(errorMutex_); + + while (!errorQueue_.empty()) { + std::string error = errorQueue_.front(); + errorQueue_.pop(); + + publishError(error); + } +} + +} // namespace custom diff --git a/src/logger.cpp b/src/logger.cpp new file mode 100644 index 0000000..cfcabe2 --- /dev/null +++ b/src/logger.cpp @@ -0,0 +1,87 @@ +#include "logger.hpp" +#include + +namespace custom { + +Logger& Logger::getInstance() { + static Logger instance; + return instance; +} + +void Logger::setLevel(LogLevel level) { + std::lock_guard lock(logMutex_); + currentLevel_ = level; +} + +void Logger::setLogFile(const std::string& filename) { + std::lock_guard lock(logMutex_); + if (logFile_) { + logFile_->close(); + } + logFile_ = std::make_unique(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 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( + 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 diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..27b0d9f --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,54 @@ +#include +#include +#include +#include "custom_robot.hpp" +#include "logger.hpp" +#include "config.hpp" + +using namespace custom; + +std::unique_ptr 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(""); + + 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; +} \ No newline at end of file diff --git a/src/mqtt.cpp b/src/mqtt.cpp new file mode 100644 index 0000000..bdf4c55 --- /dev/null +++ b/src/mqtt.cpp @@ -0,0 +1,265 @@ +#include "mqtt.hpp" +#include "logger.hpp" +#include + +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(serverURI, clientId_); + callback_ = std::make_unique(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 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 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