# 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