Add initial project structure with CMake configuration, source files, and README documentation for Unitree GO2 Custom Controller
This commit is contained in:
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
|
||||
Reference in New Issue
Block a user