diff --git a/README.md b/README.md index 8e9be5c..c9622aa 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,7 @@ A high-performance C++ implementation for controlling Unitree GO2 robot with rea - **🛡️ 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 +- **🧭 Navigation Support**: SLAM and navigation capabilities - **📊 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 @@ -22,6 +23,7 @@ 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 +- **Navigation**: Navigation and SLAM functionality - **Config**: Configuration loading, validation, and management - **Logger**: Thread-safe logging system with multiple output targets @@ -51,6 +53,9 @@ This project requires the Unitree SDK2 to be available at `../unitree_sdk2` rela ## Quick Start ```bash +# Install dependencies +./scripts/install_deps.sh + # Create build directory mkdir build && cd build @@ -60,7 +65,7 @@ cmake .. # Build the project make -j$(nproc) -# Run the robot (that's it!) +# Run the robot ./main ``` @@ -68,58 +73,39 @@ The robot will start immediately with default settings and be ready to receive M ## 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. +The system supports flexible configuration through compile-time defaults. Configuration files are optional - the system can run with compile-time defaults. -### Configuration File Structure +### Compile-time Configuration -Create `config/robot_config.json` for custom settings: +The default configuration is defined in `include/config.hpp`: -```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 - } -} +```cpp +// Network settings +constexpr std::string_view NETWORK_INTERFACE = "eth0"; + +// MQTT settings +constexpr std::string_view MQTT_BROKER = "192.168.2.236"; +constexpr int MQTT_PORT = 1883; +constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client"; +constexpr std::string_view MQTT_USERNAME = "lzwc"; +constexpr std::string_view MQTT_PASSWORD = "Lzwc@4187."; + +// Topic settings +constexpr std::string_view TOPIC_PREFIX = "unitree/go2"; +constexpr std::string_view TOPIC_CMD = "cmd"; +constexpr std::string_view TOPIC_STATE = "state"; + +// Robot control settings +constexpr double CONTROL_FREQUENCY = 200.0; // Hz +constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz + +// 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 ``` -### 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 -``` +To customize these settings, modify the values in `include/config.hpp` and rebuild the project. ## Usage @@ -127,7 +113,7 @@ Instead of creating JSON files, you can use built-in presets: ```bash # Simple execution - just run it! -./build/main +./main # That's it! No parameters needed. # The robot will start with default settings and be ready to receive MQTT commands. @@ -137,8 +123,9 @@ Instead of creating JSON files, you can use built-in presets: The robot runs with sensible defaults out of the box: -- **Network Interface**: `eth0` (auto-detected) -- **MQTT Broker**: `localhost:1883` +- **Network Interface**: `eth0` +- **MQTT Broker**: `192.168.2.236:1883` +- **MQTT Credentials**: Username `lzwc`, Password `Lzwc@4187.` - **Control Frequency**: 200Hz - **Safety Limits**: Conservative settings for safe operation - **Logging**: INFO level to console @@ -151,9 +138,10 @@ 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 +- `unitree/go2/cmd/oac`: Obstacle avoidance commands +- `unitree/go2/cmd/sport`: Sport mode commands +- `unitree/go2/cmd/rsc`: Robot state commands +- `unitree/go2/cmd/nav`: Navigation commands ### State Topics @@ -162,93 +150,117 @@ The robot communicates via MQTT using the following topic structure: - `unitree/go2/state/response`: Command execution responses - `unitree/go2/state/error`: Error messages -### Motion Commands +### Sport 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 + "cmd": "StandUp" } ``` -### Special Actions +Supported sport commands: +- `StandUp`: Stand up from lying position +- `StandDown`: Lie down from standing position +- `Sit`: Sit down +- `Damp`: Enable damping mode +- `RecoveryStand`: Recovery from emergency stop +- `BalanceStand`: Balanced standing with pose control +- `StopMove`: Stop all movement +- `Dance1`: Dance routine 1 +- `Dance2`: Dance routine 2 +- `Hello`: Greeting gesture + +### Navigation Commands ```json { "request_id": "unique_id", - "type": "special_action", - "action": "dance1", - "params": {} + "cmd": "startMapping" } ``` +Supported navigation commands: +- `startMapping`: Start SLAM mapping +- `endMapping`: End SLAM mapping +- `pauseNavigation`: Pause navigation +- `resumeNavigation`: Resume navigation +- `closeSlam`: Close SLAM service + ### System Commands ```json { "request_id": "unique_id", - "type": "system", - "command": "emergency_stop" + "cmd": "GetServiceList" } ``` +Supported system commands: +- `GetServiceList`: Get list of available services +- `SwitchService`: Enable/disable a service +- `SetReportFreq`: Set state reporting frequency + ## 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 +- `StandUp`: Stand up from lying position +- `StandDown`: Lie down from standing position +- `Sit`: Sit down +- `Damp`: Enable damping mode +- `RecoveryStand`: Recovery from emergency stop +- `BalanceStand`: Balanced standing with pose control +- `StopMove`: Stop all movement ### Special Actions -- `dance1`, `dance2`: Dance routines -- `hello`: Greeting gesture -- `stretch`: Stretching motion -- `front_flip`, `back_flip`: Acrobatic flips (use with caution) +- `Dance1`: Dance routine 1 +- `Dance2`: Dance routine 2 +- `Hello`: Greeting gesture -### Movement -- Velocity control: Linear and angular velocity commands -- Position control: Move to absolute positions -- Body pose control: Roll, pitch, yaw, and height adjustment +### Navigation Actions +- `startMapping`: Start SLAM mapping +- `endMapping`: End SLAM mapping and save map +- `pauseNavigation`: Pause navigation +- `resumeNavigation`: Resume navigation +- `closeSlam`: Close SLAM service ## 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()` +1. Add action handler in `Controller` class (`include/controller.hpp`, `src/controller.cpp`) +2. Update MQTT command processing in `CustomRobot::processSportCmd()` or appropriate processor 3. Add action documentation ### Extending MQTT API -1. Define new message types in the appropriate handler functions +1. Define new message types in the appropriate handler functions in `CustomRobot` 2. Update JSON parsing and validation 3. Add corresponding response handling ### Custom Configurations -Create specialized configuration files for different environments: +To customize the configuration, modify the values in `include/config.hpp`: +```cpp +// Example: Change MQTT broker address +constexpr std::string_view MQTT_BROKER = "192.168.1.100"; + +// Example: Change control frequency +constexpr double CONTROL_FREQUENCY = 400.0; // Hz +``` + +After making changes, rebuild the project: ```bash -# Development configuration -cp config/robot_config.json config/dev_config.json - -# Production configuration -cp config/robot_config.json config/prod_config.json +cd build +make -j$(nproc) ``` ## Quick Start Scripts @@ -276,17 +288,21 @@ unitree-go2/ │ ├── controller.hpp # Robot controller │ ├── custom_robot.hpp # Main orchestrator │ ├── logger.hpp # Logging system -│ └── mqtt.hpp # MQTT client +│ ├── mqtt.hpp # MQTT client +│ ├── navigation.hpp # Navigation and SLAM +│ └── nlohmann/ # JSON library ├── 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 +│ ├── mqtt.cpp +│ └── navigation.cpp +├── scripts/ # Utility scripts +│ ├── install_deps.sh # Install dependencies +│ └── run_robot.sh # Run with optimal settings +└── build/ # Build directory (created during build) ``` ## Troubleshooting @@ -299,7 +315,6 @@ unitree-go2/ 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: @@ -312,7 +327,7 @@ ip addr show eth0 ping 192.168.123.15 # Check if MQTT broker is accessible -telnet localhost 1883 +telnet 192.168.2.236 1883 ``` ### Performance Tuning @@ -320,38 +335,38 @@ telnet localhost 1883 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 +# Modify control frequency in include/config.hpp +# Rebuild the project +cd build +make -j$(nproc) ``` ## License -This project is licensed under the same terms as the original custom_unitree Python implementation. +This project is licensed under the MIT License - see the LICENSE file for details. ## Performance Notes -- **Control Frequency**: Default 200Hz, configurable up to 500Hz for high-performance applications +- **Control Frequency**: Default 200Hz, configurable up to 400Hz 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 +1. Fork the repository +2. Create a feature branch +3. Commit your changes +4. Push to the branch +5. Create a Pull Request ## 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 +- Support for configuration through compile-time constants - Comprehensive logging and error handling +- Navigation and SLAM capabilities ## Acknowledgments