Add initial project structure with CMake configuration, source files, and README documentation for Unitree GO2 Custom Controller
This commit is contained in:
76
.gitignore
vendored
Normal file
76
.gitignore
vendored
Normal file
@@ -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.*
|
||||
75
CMakeLists.txt
Normal file
75
CMakeLists.txt
Normal file
@@ -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}
|
||||
)
|
||||
361
README.md
361
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
|
||||
192
include/config.hpp
Normal file
192
include/config.hpp
Normal file
@@ -0,0 +1,192 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
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
|
||||
233
include/controller.hpp
Normal file
233
include/controller.hpp
Normal file
@@ -0,0 +1,233 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <chrono>
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
#include <unitree/robot/go2/sport/sport_client.hpp>
|
||||
#include <unitree/robot/go2/obstacles_avoid/obstacles_avoid_client.hpp>
|
||||
#include <unitree/robot/go2/video/video_client.hpp>
|
||||
#include <unitree/robot/go2/vui/vui_client.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
#include <unitree/idl/go2/SportModeState_.hpp>
|
||||
#include <unitree/idl/go2/LowState_.hpp>
|
||||
|
||||
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<void(const RobotState&)>;
|
||||
using ErrorCallback = std::function<void(const std::string&)>;
|
||||
|
||||
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<unitree::robot::go2::SportClient> sportClient_;
|
||||
std::unique_ptr<unitree::robot::go2::ObstaclesAvoidClient> obstacleClient_;
|
||||
std::unique_ptr<unitree::robot::go2::VideoClient> videoClient_;
|
||||
std::unique_ptr<unitree::robot::go2::VuiClient> vuiClient_;
|
||||
|
||||
// State subscribers
|
||||
std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>> sportStateSubscriber_;
|
||||
std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>> lowStateSubscriber_;
|
||||
|
||||
// Current state
|
||||
mutable std::mutex stateMutex_;
|
||||
RobotState currentState_;
|
||||
|
||||
// Command queue
|
||||
std::queue<MotionCommand> commandQueue_;
|
||||
std::mutex commandMutex_;
|
||||
std::condition_variable commandCondition_;
|
||||
|
||||
// Control threads
|
||||
std::thread controlThread_;
|
||||
std::thread stateThread_;
|
||||
std::atomic<bool> running_;
|
||||
std::atomic<bool> initialized_;
|
||||
|
||||
// Callbacks
|
||||
StateCallback stateCallback_;
|
||||
ErrorCallback errorCallback_;
|
||||
|
||||
// Safety
|
||||
std::chrono::steady_clock::time_point lastCommandTime_;
|
||||
std::atomic<bool> emergencyStopActive_;
|
||||
|
||||
// Network
|
||||
std::string networkInterface_;
|
||||
|
||||
// Timing
|
||||
std::chrono::milliseconds controlPeriod_;
|
||||
std::chrono::milliseconds statePeriod_;
|
||||
};
|
||||
|
||||
} // namespace custom
|
||||
123
include/custom_robot.hpp
Normal file
123
include/custom_robot.hpp
Normal file
@@ -0,0 +1,123 @@
|
||||
#pragma once
|
||||
|
||||
#include "controller.hpp"
|
||||
#include "mqtt.hpp"
|
||||
#include "config.hpp"
|
||||
#include "logger.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
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<Controller> robotController_;
|
||||
std::unique_ptr<MqttClient> mqttClient_;
|
||||
|
||||
// Configuration
|
||||
std::string configFile_;
|
||||
|
||||
// State
|
||||
std::atomic<bool> running_;
|
||||
std::atomic<bool> initialized_;
|
||||
std::atomic<bool> mqttConnected_;
|
||||
|
||||
// Threads
|
||||
std::thread mainThread_;
|
||||
std::thread safetyThread_;
|
||||
std::vector<std::thread> 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<std::string, std::chrono::steady_clock::time_point> pendingRequests_;
|
||||
|
||||
// Error handling
|
||||
std::mutex errorMutex_;
|
||||
std::queue<std::string> errorQueue_;
|
||||
void processErrorQueue();
|
||||
};
|
||||
|
||||
} // namespace custom
|
||||
53
include/logger.hpp
Normal file
53
include/logger.hpp
Normal file
@@ -0,0 +1,53 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <sstream>
|
||||
#include <chrono>
|
||||
#include <iomanip>
|
||||
|
||||
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<std::ofstream> 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
|
||||
97
include/mqtt.hpp
Normal file
97
include/mqtt.hpp
Normal file
@@ -0,0 +1,97 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <queue>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <mqtt/async_client.h>
|
||||
|
||||
namespace custom {
|
||||
|
||||
class MqttClient {
|
||||
public:
|
||||
using MessageCallback = std::function<void(const std::string& topic, const std::string& payload)>;
|
||||
using ConnectionCallback = std::function<void(bool connected)>;
|
||||
|
||||
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<mqtt::async_client> client_;
|
||||
std::unique_ptr<Callback> callback_;
|
||||
|
||||
MessageCallback messageCallback_;
|
||||
ConnectionCallback connectionCallback_;
|
||||
|
||||
std::atomic<bool> connected_;
|
||||
std::atomic<bool> reconnecting_;
|
||||
|
||||
// Message processing
|
||||
std::queue<QueuedMessage> messageQueue_;
|
||||
std::mutex queueMutex_;
|
||||
std::condition_variable queueCondition_;
|
||||
std::thread messageProcessor_;
|
||||
std::atomic<bool> processorRunning_;
|
||||
|
||||
// Connection parameters
|
||||
std::string broker_;
|
||||
int port_;
|
||||
std::string clientId_;
|
||||
std::string username_;
|
||||
std::string password_;
|
||||
|
||||
// Reconnection
|
||||
std::thread reconnectThread_;
|
||||
std::atomic<bool> shouldReconnect_;
|
||||
int reconnectDelay_ = 5; // seconds
|
||||
};
|
||||
|
||||
} // namespace custom
|
||||
53
scripts/install_deps.sh
Normal file
53
scripts/install_deps.sh
Normal file
@@ -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)"
|
||||
188
scripts/run_robot.sh
Normal file
188
scripts/run_robot.sh
Normal file
@@ -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
|
||||
316
src/config.cpp
Normal file
316
src/config.cpp
Normal file
@@ -0,0 +1,316 @@
|
||||
#include "config.hpp"
|
||||
#include "logger.hpp"
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
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
|
||||
575
src/controller.cpp
Normal file
575
src/controller.cpp
Normal file
@@ -0,0 +1,575 @@
|
||||
#include "controller.hpp"
|
||||
#include "config.hpp"
|
||||
#include "logger.hpp"
|
||||
#include <unitree/robot/channel/channel_factory.hpp>
|
||||
#include <unitree/common/thread/thread.hpp>
|
||||
|
||||
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<int>(1000.0 / config.control_frequency));
|
||||
statePeriod_ = std::chrono::milliseconds(static_cast<int>(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<unitree::robot::go2::SportClient>();
|
||||
sportClient_->SetTimeout(10.0f);
|
||||
sportClient_->Init();
|
||||
|
||||
// Initialize obstacle avoidance client
|
||||
obstacleClient_ = std::make_unique<unitree::robot::go2::ObstaclesAvoidClient>();
|
||||
obstacleClient_->Init();
|
||||
|
||||
// Initialize video client
|
||||
videoClient_ = std::make_unique<unitree::robot::go2::VideoClient>();
|
||||
videoClient_->Init();
|
||||
|
||||
// Initialize VUI client
|
||||
vuiClient_ = std::make_unique<unitree::robot::go2::VuiClient>();
|
||||
vuiClient_->Init();
|
||||
|
||||
// Initialize state subscribers
|
||||
sportStateSubscriber_.reset(
|
||||
new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>("rt/sportmodestate")
|
||||
);
|
||||
sportStateSubscriber_->InitChannel(
|
||||
std::bind(&Controller::updateSportModeState, this, std::placeholders::_1), 1
|
||||
);
|
||||
|
||||
lowStateSubscriber_.reset(
|
||||
new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>("rt/lowstate")
|
||||
);
|
||||
lowStateSubscriber_->InitChannel(
|
||||
std::bind(&Controller::updateLowLevelState, this, std::placeholders::_1), 1
|
||||
);
|
||||
|
||||
// Initialize robot state
|
||||
{
|
||||
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<const unitree_go::msg::dds_::SportModeState_*>(message);
|
||||
|
||||
std::lock_guard<std::mutex> 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<const unitree_go::msg::dds_::LowState_*>(message);
|
||||
|
||||
std::lock_guard<std::mutex> 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<std::chrono::seconds>(
|
||||
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<std::mutex> 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
|
||||
682
src/custom_robot.cpp
Normal file
682
src/custom_robot.cpp
Normal file
@@ -0,0 +1,682 @@
|
||||
#include "custom_robot.hpp"
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
|
||||
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<Controller>(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<MqttClient>(
|
||||
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::milliseconds>(
|
||||
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<std::mutex> lock(stateCacheMutex_);
|
||||
lastKnownState_ = state;
|
||||
}
|
||||
|
||||
// Publish state if enough time has passed
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto timeSinceLastPublish = std::chrono::duration<double>(now - lastStatePublish_).count();
|
||||
|
||||
if (timeSinceLastPublish >= statePublishInterval_) {
|
||||
publishState();
|
||||
lastStatePublish_ = now;
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::handleRobotError(const std::string& error) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(errorMutex_);
|
||||
errorQueue_.push(error);
|
||||
}
|
||||
|
||||
stats_.errorsOccurred++;
|
||||
}
|
||||
|
||||
void CustomRobot::publishState() {
|
||||
if (!mqttClient_ || !mqttClient_->isConnected()) {
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
RobotState state;
|
||||
{
|
||||
std::lock_guard<std::mutex> 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::milliseconds>(
|
||||
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::milliseconds>(
|
||||
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<std::chrono::milliseconds>(
|
||||
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<double>(state.motor_positions, state.motor_positions + 12);
|
||||
json["motors"]["velocities"] = std::vector<double>(state.motor_velocities, state.motor_velocities + 12);
|
||||
json["motors"]["torques"] = std::vector<double>(state.motor_torques, state.motor_torques + 12);
|
||||
json["motors"]["temperatures"] = std::vector<double>(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<std::chrono::seconds>(now - stats_.startTime).count();
|
||||
status["uptime_seconds"] = uptime;
|
||||
|
||||
status["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
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<double>(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<double>(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<std::mutex> lock(errorMutex_);
|
||||
|
||||
while (!errorQueue_.empty()) {
|
||||
std::string error = errorQueue_.front();
|
||||
errorQueue_.pop();
|
||||
|
||||
publishError(error);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace custom
|
||||
87
src/logger.cpp
Normal file
87
src/logger.cpp
Normal file
@@ -0,0 +1,87 @@
|
||||
#include "logger.hpp"
|
||||
#include <iostream>
|
||||
|
||||
namespace custom {
|
||||
|
||||
Logger& Logger::getInstance() {
|
||||
static Logger instance;
|
||||
return instance;
|
||||
}
|
||||
|
||||
void Logger::setLevel(LogLevel level) {
|
||||
std::lock_guard<std::mutex> lock(logMutex_);
|
||||
currentLevel_ = level;
|
||||
}
|
||||
|
||||
void Logger::setLogFile(const std::string& filename) {
|
||||
std::lock_guard<std::mutex> lock(logMutex_);
|
||||
if (logFile_) {
|
||||
logFile_->close();
|
||||
}
|
||||
logFile_ = std::make_unique<std::ofstream>(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<std::mutex> 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<std::chrono::milliseconds>(
|
||||
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
|
||||
54
src/main.cpp
Normal file
54
src/main.cpp
Normal file
@@ -0,0 +1,54 @@
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <signal.h>
|
||||
#include "custom_robot.hpp"
|
||||
#include "logger.hpp"
|
||||
#include "config.hpp"
|
||||
|
||||
using namespace custom;
|
||||
|
||||
std::unique_ptr<CustomRobot> 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<CustomRobot>("");
|
||||
|
||||
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;
|
||||
}
|
||||
265
src/mqtt.cpp
Normal file
265
src/mqtt.cpp
Normal file
@@ -0,0 +1,265 @@
|
||||
#include "mqtt.hpp"
|
||||
#include "logger.hpp"
|
||||
#include <sstream>
|
||||
|
||||
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<mqtt::async_client>(serverURI, clientId_);
|
||||
callback_ = std::make_unique<Callback>(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<std::mutex> 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<std::mutex> 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
|
||||
Reference in New Issue
Block a user