376 lines
11 KiB
Markdown
376 lines
11 KiB
Markdown
# 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
|
|
- **🧭 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
|
|
- **🔧 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
|
|
- **Navigation**: Navigation and SLAM functionality
|
|
- **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
|
|
# Install dependencies
|
|
./scripts/install_deps.sh
|
|
|
|
# Create build directory
|
|
mkdir build && cd build
|
|
|
|
# Configure with CMake
|
|
cmake ..
|
|
|
|
# Build the project
|
|
make -j$(nproc)
|
|
|
|
# Run the robot
|
|
./main
|
|
```
|
|
|
|
The robot will start immediately with default settings and be ready to receive MQTT commands!
|
|
|
|
## Configuration
|
|
|
|
The system supports flexible configuration through compile-time defaults. Configuration files are optional - the system can run with compile-time defaults.
|
|
|
|
### Compile-time Configuration
|
|
|
|
The default configuration is defined in `include/config.hpp`:
|
|
|
|
```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
|
|
```
|
|
|
|
To customize these settings, modify the values in `include/config.hpp` and rebuild the project.
|
|
|
|
## Usage
|
|
|
|
### Basic Usage
|
|
|
|
```bash
|
|
# Simple execution - just run it!
|
|
./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`
|
|
- **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
|
|
|
|
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/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
|
|
|
|
- `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
|
|
|
|
### Sport Commands
|
|
|
|
```json
|
|
{
|
|
"request_id": "unique_id",
|
|
"cmd": "StandUp"
|
|
}
|
|
```
|
|
|
|
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",
|
|
"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",
|
|
"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
|
|
- `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`: Dance routine 1
|
|
- `Dance2`: Dance routine 2
|
|
- `Hello`: Greeting gesture
|
|
|
|
### 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
|
|
- **Connection Monitoring**: Automatic reconnection and error handling
|
|
|
|
## Development
|
|
|
|
### Adding New Actions
|
|
|
|
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 in `CustomRobot`
|
|
2. Update JSON parsing and validation
|
|
3. Add corresponding response handling
|
|
|
|
### Custom Configurations
|
|
|
|
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
|
|
cd build
|
|
make -j$(nproc)
|
|
```
|
|
|
|
## 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
|
|
│ ├── navigation.hpp # Navigation and SLAM
|
|
│ └── nlohmann/ # JSON library
|
|
├── src/ # Source files
|
|
│ ├── config.cpp
|
|
│ ├── controller.cpp
|
|
│ ├── custom_robot.cpp
|
|
│ ├── logger.cpp
|
|
│ ├── main.cpp
|
|
│ ├── 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
|
|
|
|
### 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 192.168.2.236 1883
|
|
```
|
|
|
|
### Performance Tuning
|
|
|
|
For optimal performance:
|
|
|
|
```bash
|
|
# Modify control frequency in include/config.hpp
|
|
# Rebuild the project
|
|
cd build
|
|
make -j$(nproc)
|
|
```
|
|
|
|
## License
|
|
|
|
This project is licensed under the MIT License - see the LICENSE file for details.
|
|
|
|
## Performance Notes
|
|
|
|
- **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. 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 through compile-time constants
|
|
- Comprehensive logging and error handling
|
|
- Navigation and SLAM capabilities
|
|
|
|
## 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 |