# 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
- **🎭 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`:



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:



Configuration is done at compile time by modifying values in `include/config.hpp`.
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`: All commands (sport, navigation, robot state, obstacle avoidance, motion switcher)

Commands are sent to the command topic with a JSON payload specifying the command type and parameters.



### Sport Commands

Sport commands are sent with a JSON payload containing the command type and optional parameters:

```json
{
  "type": "sport",
  "cmd": "StandUp",
  "param": {
    // Optional parameters depending on the command
  }
}
```

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
- `Move`: Move with specified velocities (vx, vy, vyaw)
- `Euler`: Set body orientation (roll, pitch, yaw)
- `RiseSit`: Rise and sit motion
- `SpeedLevel`: Set movement speed level
- `Stretch`: Stretching motion
- `SwitchJoystick`: Enable/disable joystick control
- `Content`: Content expression
- `Heart`: Heart gesture
- `Pose`: Pose control
- `Scrape`: Scrape motion
- `FrontFlip`: Front flip trick
- `FrontJump`: Front jump motion
- `FrontPounce`: Front pounce motion
- `LeftFlip`: Left flip trick
- `BackFlip`: Back flip trick
- `HandStand`: Handstand motion
- `FreeWalk`: Free walking mode
- `FreeBound`: Free bounding mode
- `FreeJump`: Free jumping mode
- `FreeAvoid`: Free obstacle avoidance
- `ClassicWalk`: Classic walking mode
- `WalkUpright`: Upright walking mode
- `CrossStep`: Cross step motion
- `AutoRecoverSet`: Set auto recovery mode
- `StaticWalk`: Static walking mode
- `TrotRun`: Trot running mode
- `EconomicGait`: Economic gait mode
- `SwitchAvoidMode`: Switch obstacle avoidance mode
- `BodyHeight`: Adjust body height
- `SwitchGait`: Switch gait type
- `TrajectoryFollow`: Follow a trajectory path
- `ContinuousGait`: Continuous gait mode
- `MoveToPos`: Move to specific position
- `FastWalk`: Fast walking mode
- `FootRaiseHeight`: Adjust foot raise height
- And more...

### Navigation Commands

Navigation commands are sent with a JSON payload containing the command type and parameters:

```json
{
  "type": "nav",
  "cmd": "startMapping"
}
```

Supported navigation commands:
- `start_mapping`: Start SLAM mapping
- `end_mapping`: End SLAM mapping with map address parameter
- `initialize_pose`: Set initial pose with position and orientation
- `pose_navigation`: Navigate to a specific pose
- `pause_navigation`: Pause navigation
- `resume_navigation`: Resume navigation
- `close_slam`: Close SLAM service

### Obstacle Avoidance Commands

Obstacle avoidance commands are sent with a JSON payload containing the command type and parameters:

```json
{
  "type": "oac",
  "cmd": "SwitchSet",
  "param": {
    "enable": true
  }
}
```

Supported obstacle avoidance commands:
- `SwitchSet`: Enable/disable obstacle avoidance
- `SwitchGet`: Get obstacle avoidance status
- `UseRemoteCommandFromApi`: Configure remote command source
- `MoveToAbsolutePosition`: Move to absolute position
- `MoveToIncrementPosition`: Move to increment position

### Motion Switcher Commands

Motion switcher commands allow switching between different motion modes of the robot:

```json
{
  "type": "msc",
  "cmd": "SelectMode",
  "param": {
    "mode": "ai"
  }
}
```

Supported motion switcher commands:
- `CheckMode`: Check the current motion mode and robot form
- `SelectMode`: Select a specific motion mode (normal, advanced, ai, ai-w, normal-w)
- `ReleaseMode`: Release the current motion mode

#### CheckMode

Check the current motion mode and robot form.

Request:
```json
{
  "type": "msc",
  "cmd": "CheckMode"
}
```

Response:
```json
{
  "code": 0,
  "data": {
    "form": "0",
    "name": "normal"
  }
}
```

#### SelectMode

Select a specific motion mode. Available modes:
- `normal`: Standard walking mode
- `advanced`: Enhanced walking capabilities
- `ai`: AI-powered motion mode
- `ai-w`: AI mode with specific walking pattern
- `normal-w`: Normal mode with specific walking pattern

Request:
```json
{
  "type": "msc",
  "cmd": "SelectMode",
  "param": {
    "mode": "ai"
  }
}
```

Response:
```json
{
  "code": 0,
  "data": {
    "form": "0",
    "name": "ai"
  }
}
```

#### ReleaseMode

Release the current motion mode and return to default mode.

Request:
```json
{
  "type": "msc",
  "cmd": "ReleaseMode"
}
```

Response:
```json
{
  "code": 0,
  "data": {
    "form": "0",
    "name": "normal"
  }
}
```

### System Commands

System commands are sent with a JSON payload containing the command type and parameters:

```json
{
  "type": "rsc",
  "cmd": "GetServiceList"
}
```

Supported system commands:
- `list`: Get list of available services
- `switch`: Enable/disable a service

## 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

### Motion Switcher Actions
- `CheckMode`: Check the current motion mode and robot form. Returns the current motion mode and robot form information.
- `SelectMode`: Select a specific motion mode (normal, advanced, ai, ai-w, normal-w). Switches the robot to the specified motion mode.
- `ReleaseMode`: Release the current motion mode and return to default mode. Releases the currently selected motion mode.

### Navigation Actions
- `start_mapping`: Start SLAM mapping
- `end_mapping`: End SLAM mapping and save map
- `pause_navigation`: Pause navigation
- `resume_navigation`: Resume navigation
- `close_slam`: 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

### Adding New Motion Switcher Actions

1. Add action handler in `Controller` class (`include/controller.hpp`, `src/controller.cpp`) under MotionSwitcher section
2. Update MQTT command processing in `CustomRobot::processMscCmd()`
3. Add action documentation in the Motion Switcher Actions section

### 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

The robot's configuration is defined at compile time in `include/config.hpp`. To customize the configuration:

1. Edit the constants in `include/config.hpp` to change default values
2. Rebuild the project:
   ```bash
   cd build
   make -j$(nproc)
   ```

Common configuration options include:
- Network interface
- MQTT broker settings
- Topic prefixes
- Control frequencies
- Safety limits

## 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
├── .gitignore              # Git ignore file
├── 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          # Configuration implementation
│   ├── controller.cpp      # Robot controller implementation
│   ├── custom_robot.cpp    # Main orchestrator implementation
│   ├── logger.cpp          # Logging system implementation
│   ├── main.cpp            # Entry point
│   ├── mqtt.cpp            # MQTT client implementation
│   └── navigation.cpp      # Navigation and SLAM implementation
├── 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
- **Memory Usage**: Optimized for minimal heap allocations during runtime
- **CPU Usage**: Multi-threaded design with separate threads for control and MQTT

## 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