diff --git a/CMakeLists.txt b/CMakeLists.txt index f2eacea..7a50671 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ set(SOURCES src/custom_robot.cpp src/config.cpp src/logger.cpp + # src/mqtt.cpp # Disabled MQTT for now ) # Create executable with simple name diff --git a/include/config.hpp b/include/config.hpp index 513b800..7aa0d4b 100644 --- a/include/config.hpp +++ b/include/config.hpp @@ -3,205 +3,101 @@ #include #include -namespace customConfig { +namespace custom { -// Configuration template selector -enum class ConfigPreset { - Default, - HighPerformance, - Development, - Safety -}; - -// Motion gait enum +/** + * @brief Robot motion gait types + */ enum class Gait : int { - IDLE = 0, - TROT = 1, - TROT_RUNNING = 2 + IDLE = 0, // Standing still + TROT = 1, // Normal walking + TROT_RUNNING = 2 // Fast running }; -// Network configuration + +// Network settings constexpr std::string_view NETWORK_INTERFACE = "eth0"; -// MQTT configuration +// MQTT settings constexpr std::string_view MQTT_BROKER = "localhost"; constexpr int MQTT_PORT = 1883; +constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client"; constexpr std::string_view MQTT_USERNAME = ""; constexpr std::string_view MQTT_PASSWORD = ""; -constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client"; -// Topic configuration +// Topic settings constexpr std::string_view TOPIC_PREFIX = "unitree/go2"; constexpr std::string_view TOPIC_CMD = "cmd"; constexpr std::string_view TOPIC_STATE = "state"; constexpr std::string_view TOPIC_VIDEO = "video"; constexpr std::string_view TOPIC_AUDIO = "audio"; -// Robot control configuration -constexpr double CONTROL_FREQUENCY = 200.0; // Hz -constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz +// Robot control settings +constexpr double CONTROL_FREQUENCY = 200.0; // Hz +constexpr double STATE_PUBLISH_FREQUENCY = 50.0; // Hz -// Safety configuration -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 +// 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 configuration -constexpr double STAND_HEIGHT = 0.0; // relative height +// Motion settings +constexpr double STAND_HEIGHT = 0.0; // relative height constexpr Gait DEFAULT_GAIT = Gait::IDLE; -// Video configuration +// Video settings constexpr int VIDEO_WIDTH = 1280; constexpr int VIDEO_HEIGHT = 720; constexpr int VIDEO_FPS = 30; constexpr std::string_view VIDEO_FORMAT = "h264"; constexpr bool VIDEO_ENABLED = true; -// Audio configuration +// Audio settings constexpr int AUDIO_SAMPLE_RATE = 16000; constexpr int AUDIO_CHANNELS = 1; constexpr std::string_view AUDIO_FORMAT = "pcm"; constexpr bool AUDIO_ENABLED = true; -// Configuration presets using template specialization -template -struct ConfigParams; +class CustomConfig { +public: + // 网络和MQTT连接 + std::string network_interface; + std::string mqtt_broker; + int mqtt_port; + std::string mqtt_username; + std::string mqtt_password; + std::string mqtt_client_id; + + // MQTT话题 + std::string topic_prefix; + std::string topic_cmd; + std::string topic_state; + std::string topic_video; + std::string topic_audio; + + // 机器人控制和安全 + double control_frequency; + double state_publish_frequency; + double max_linear_velocity; + double max_angular_velocity; + double emergency_stop_timeout; + double stand_height; + int default_gait; + + // 多媒体 + bool enable_video; + bool enable_audio; + int video_width; + int video_height; + int video_fps; + std::string video_format; + int audio_sample_rate; + int audio_channels; + std::string audio_format; + + void loadDefaults(); -// Default preset -template<> -struct ConfigParams { - static constexpr double control_frequency = CONTROL_FREQUENCY; - static constexpr double state_publish_frequency = STATE_PUBLISH_FREQUENCY; - static constexpr double max_linear_velocity = MAX_LINEAR_VELOCITY; - static constexpr double max_angular_velocity = MAX_ANGULAR_VELOCITY; - static constexpr double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT; - static constexpr bool video_enabled = VIDEO_ENABLED; - static constexpr bool audio_enabled = AUDIO_ENABLED; }; -// High performance preset -template<> -struct ConfigParams { - 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 double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT; - static constexpr bool video_enabled = false; // Disabled for performance - static constexpr bool audio_enabled = false; // Disabled for performance -}; -// Development preset -template<> -struct ConfigParams { - 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 double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT; - static constexpr bool video_enabled = true; - static constexpr bool audio_enabled = true; -}; - -// Safety preset -template<> -struct ConfigParams { - 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 video_enabled = true; - static constexpr bool audio_enabled = true; -}; - -// Compile-time configuration selection (set this to choose preset) -constexpr ConfigPreset ACTIVE_PRESET = ConfigPreset::Default; - -// Compile-time configuration struct -template -struct CompileTimeConfig { - // Use selected preset parameters - using params = ConfigParams

; - - // Network settings - static constexpr std::string_view network_interface = NETWORK_INTERFACE; - - // MQTT settings - static constexpr std::string_view mqtt_broker = MQTT_BROKER; - static constexpr int mqtt_port = MQTT_PORT; - static constexpr std::string_view mqtt_username = MQTT_USERNAME; - static constexpr std::string_view mqtt_password = MQTT_PASSWORD; - static constexpr std::string_view mqtt_client_id = MQTT_CLIENT_ID; - - // Topics - static constexpr std::string_view topic_prefix = TOPIC_PREFIX; - static constexpr std::string_view cmd_topic = TOPIC_CMD; - static constexpr std::string_view state_topic = TOPIC_STATE; - static constexpr std::string_view video_topic = TOPIC_VIDEO; - static constexpr std::string_view audio_topic = TOPIC_AUDIO; - - // Robot settings (from preset) - static constexpr double control_frequency = params::control_frequency; - static constexpr double state_publish_frequency = params::state_publish_frequency; - static constexpr bool enable_video = params::video_enabled; - static constexpr bool enable_audio = params::audio_enabled; - - // Safety settings (from preset) - static constexpr double max_linear_velocity = params::max_linear_velocity; - static constexpr double max_angular_velocity = params::max_angular_velocity; - static constexpr double emergency_stop_timeout = params::emergency_stop_timeout; - - // Motion settings - static constexpr double stand_height = STAND_HEIGHT; - static constexpr Gait default_gait = DEFAULT_GAIT; - - // Video settings - static constexpr int video_width = VIDEO_WIDTH; - static constexpr int video_height = VIDEO_HEIGHT; - static constexpr int video_fps = VIDEO_FPS; - static constexpr std::string_view video_format = VIDEO_FORMAT; - - // Audio settings - static constexpr int audio_sample_rate = AUDIO_SAMPLE_RATE; - static constexpr int audio_channels = AUDIO_CHANNELS; - static constexpr std::string_view audio_format = AUDIO_FORMAT; - - // Utility functions - static std::string getFullTopic(std::string_view topic) { - return std::string(topic_prefix) + "/" + std::string(topic); - } -}; - -// Type alias for current configuration -using RobotConfig = CompileTimeConfig; - -// Configuration validation utilities -template -constexpr bool isConfigValid() { - using config = ConfigParams

; - return config::control_frequency > 0.0 && - config::state_publish_frequency > 0.0 && - config::max_linear_velocity > 0.0 && - config::max_angular_velocity > 0.0 && - config::emergency_stop_timeout > 0.0; -} - -// Compile-time validation -static_assert(isConfigValid(), "Invalid configuration parameters"); - -// Configuration preset name utilities -template -constexpr const char* getPresetName() { - if constexpr (P == ConfigPreset::Default) return "Default"; - else if constexpr (P == ConfigPreset::HighPerformance) return "HighPerformance"; - else if constexpr (P == ConfigPreset::Development) return "Development"; - else if constexpr (P == ConfigPreset::Safety) return "Safety"; - else return "Unknown"; -} - -constexpr const char* getActivePresetName() { - return getPresetName(); -} - -} // namespace customConfig +} // namespace custom diff --git a/include/controller.hpp b/include/controller.hpp index fa7fb0b..5f4fd29 100644 --- a/include/controller.hpp +++ b/include/controller.hpp @@ -17,19 +17,19 @@ namespace custom { class Controller { public: - explicit Controller(const std::string& networkInterface); + Controller(); ~Controller(); bool initialize(); bool start(); bool stop(); + void shutdown() { stop(); } bool isRunning() const { return running_; } // Sport bool StandUp(); bool StandDown(); bool Sit(); - bool Lie(); bool Damp(); bool RecoveryStand(); bool StopMove(); @@ -47,12 +47,11 @@ public: private: - std::unique_ptr sportClient_; - std::unique_ptr obstacleClient_; + std::unique_ptr sc_; + std::unique_ptr oac_; std::atomic running_; std::atomic initialized_; - std::string networkInterface_; }; } // namespace custom diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index 760fbdc..c4263b6 100644 --- a/include/custom_robot.hpp +++ b/include/custom_robot.hpp @@ -6,6 +6,8 @@ #include #include +#include +#include #include namespace custom { @@ -18,6 +20,8 @@ public: bool initialize(); bool start(); + Controller* getController() const { return controller_.get(); } + // Robot state methods bool GetServiceList(std::vector& serviceList); bool SwitchService(const std::string& serviceName, bool enable); @@ -25,8 +29,9 @@ public: private: std::unique_ptr controller_; - std::unique_ptr stateClient_; + std::unique_ptr rsc_; + CustomConfig config_; std::atomic running_; std::atomic initialized_; }; diff --git a/src/config.cpp b/src/config.cpp index 3d502ec..96a0ac0 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -1,319 +1,42 @@ #include "config.hpp" -#include "logger.hpp" -#include -#include -namespace customConfig { +namespace custom { -Config::Config() { - // Load default configuration - loadDefaults(); +void CustomConfig::loadDefaults() { + // 网络和MQTT连接 + network_interface = std::string(NETWORK_INTERFACE); + mqtt_broker = std::string(MQTT_BROKER); + mqtt_port = MQTT_PORT; + mqtt_username = std::string(MQTT_USERNAME); + mqtt_password = std::string(MQTT_PASSWORD); + mqtt_client_id = std::string(MQTT_CLIENT_ID); + + // MQTT话题 + topic_prefix = std::string(TOPIC_PREFIX); + topic_cmd = std::string(TOPIC_CMD); + topic_state = std::string(TOPIC_STATE); + topic_video = std::string(TOPIC_VIDEO); + topic_audio = std::string(TOPIC_AUDIO); + + // 机器人控制和安全 + control_frequency = CONTROL_FREQUENCY; + state_publish_frequency = STATE_PUBLISH_FREQUENCY; + max_linear_velocity = MAX_LINEAR_VELOCITY; + max_angular_velocity = MAX_ANGULAR_VELOCITY; + emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT; + stand_height = STAND_HEIGHT; + default_gait = static_cast(DEFAULT_GAIT); + + // 多媒体 + enable_video = VIDEO_ENABLED; + enable_audio = AUDIO_ENABLED; + video_width = VIDEO_WIDTH; + video_height = VIDEO_HEIGHT; + video_fps = VIDEO_FPS; + video_format = std::string(VIDEO_FORMAT); + audio_sample_rate = AUDIO_SAMPLE_RATE; + audio_channels = AUDIO_CHANNELS; + audio_format = std::string(AUDIO_FORMAT); } -Config& Config::getInstance() { - static Config instance; - return instance; -} - -void Config::loadDefaults() { - 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 = VIDEO_ENABLED; - config_.enable_audio = AUDIO_ENABLED; - - 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 = static_cast(DEFAULT_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(); - using hp = ConfigParams; - config_.control_frequency = hp::control_frequency; - config_.state_publish_frequency = hp::state_publish_frequency; - config_.max_linear_velocity = hp::max_linear_velocity; - config_.max_angular_velocity = hp::max_angular_velocity; - config_.enable_video = hp::video_enabled; - config_.enable_audio = hp::audio_enabled; -} - -void Config::loadDevelopmentPreset() { - loadDefaults(); - using dev = ConfigParams; - config_.control_frequency = dev::control_frequency; - config_.state_publish_frequency = dev::state_publish_frequency; - config_.max_linear_velocity = dev::max_linear_velocity; - config_.max_angular_velocity = dev::max_angular_velocity; - config_.enable_video = dev::video_enabled; - config_.enable_audio = dev::audio_enabled; -} - -void Config::loadSafetyPreset() { - loadDefaults(); - using safe = ConfigParams; - config_.control_frequency = safe::control_frequency; - config_.state_publish_frequency = safe::state_publish_frequency; - config_.max_linear_velocity = safe::max_linear_velocity; - config_.max_angular_velocity = safe::max_angular_velocity; - config_.emergency_stop_timeout = safe::emergency_stop_timeout; - config_.enable_video = safe::video_enabled; - config_.enable_audio = safe::audio_enabled; -} - -bool Config::loadConfig(const std::string& configFile) { - // For now just load defaults, JSON loading can be added later if needed - loadDefaults(); - - /* TODO: Add JSON loading support if needed - try { - std::ifstream file(configFile); - if (!file.is_open()) { - LOG_WARN("Config file not found: " + configFile + ", using defaults"); - loadDefaults(); - 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())); - loadDefaults(); - return false; - } - */ - - return true; -} - -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 = 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 = 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 = 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 = 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 = 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 = VIDEO_WIDTH; - config_.video_height = VIDEO_HEIGHT; - valid = false; - } - - if (config_.video_fps <= 0 || config_.video_fps > 120) { - LOG_WARN("Invalid video FPS, setting to default"); - config_.video_fps = 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 = AUDIO_SAMPLE_RATE; - valid = false; - } - - if (config_.audio_channels <= 0) { - LOG_WARN("Invalid audio channels, setting to default"); - config_.audio_channels = AUDIO_CHANNELS; - valid = false; - } - - return valid; -} - -} // namespace customConfig +} // namespace custom \ No newline at end of file diff --git a/src/controller.cpp b/src/controller.cpp index a523707..edbb329 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -1,13 +1,13 @@ #include "controller.hpp" #include "config.hpp" #include "logger.hpp" -#include #include namespace custom { -Controller::Controller(const std::string& networkInterface) - : networkInterface_(networkInterface), running_(false), initialized_(false) { +Controller::Controller() + : running_(false), initialized_(false) { + LOG_INFO("Controller created"); } Controller::~Controller() { @@ -16,24 +16,20 @@ Controller::~Controller() { } LOG_INFO("Shutting down robot controller"); - sportClient_.reset(); - obstacleClient_.reset(); + sc_.reset(); + oac_.reset(); initialized_ = false; } bool Controller::initialize() { try { - LOG_INFO("Initializing robot controller with interface: " + networkInterface_); + sc_ = std::make_unique(); + sc_->SetTimeout(10.0f); + sc_->Init(); - unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface_); - - sportClient_ = std::make_unique(); - sportClient_->SetTimeout(10.0f); - sportClient_->Init(); - - obstacleClient_ = std::make_unique(); - obstacleClient_->SetTimeout(3.0f); - obstacleClient_->Init(); + oac_ = std::make_unique(); + oac_->SetTimeout(3.0f); + oac_->Init(); initialized_ = true; @@ -70,8 +66,6 @@ bool Controller::stop() { return true; } - LOG_INFO("Stopping robot controller"); - running_ = false; LOG_INFO("Robot controller stopped"); @@ -84,7 +78,7 @@ bool Controller::stop() { bool Controller::StandUp() { LOG_INFO("Standing up"); try { - return sportClient_ && sportClient_->StandUp() == 0; + return sc_ && sc_->StandUp() == 0; } catch (const std::exception& e) { LOG_ERROR("Stand up failed: " + std::string(e.what())); return false; @@ -94,7 +88,7 @@ bool Controller::StandUp() { bool Controller::StandDown() { LOG_INFO("Standing down"); try { - return sportClient_ && sportClient_->StandDown() == 0; + return sc_ && sc_->StandDown() == 0; } catch (const std::exception& e) { LOG_ERROR("Stand down failed: " + std::string(e.what())); return false; @@ -104,27 +98,17 @@ bool Controller::StandDown() { bool Controller::Sit() { LOG_INFO("Sitting"); try { - return sportClient_ && sportClient_->Sit() == 0; + return sc_ && sc_->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; + return sc_ && sc_->Damp() == 0; } catch (const std::exception& e) { LOG_ERROR("Damp failed: " + std::string(e.what())); return false; @@ -135,8 +119,8 @@ bool Controller::RecoveryStand() { LOG_INFO("Executing recovery stand"); try { - if (sportClient_) { - int32_t result = sportClient_->RecoveryStand(); + if (sc_) { + int32_t result = sc_->RecoveryStand(); return result == 0; } return false; @@ -148,7 +132,7 @@ bool Controller::RecoveryStand() { bool Controller::StopMove() { try { - return sportClient_ && sportClient_->StopMove() == 0; + return sc_ && sc_->StopMove() == 0; } catch (const std::exception& e) { LOG_ERROR("Stop move failed: " + std::string(e.what())); return false; @@ -157,7 +141,7 @@ bool Controller::StopMove() { bool Controller::BalanceStand() { try { - return sportClient_ && sportClient_->BalanceStand() == 0; + return sc_ && sc_->BalanceStand() == 0; } catch (const std::exception& e) { LOG_ERROR("Balance stand failed: " + std::string(e.what())); return false; @@ -166,7 +150,7 @@ bool Controller::BalanceStand() { bool Controller::Dance1() { try { - return sportClient_ && sportClient_->Dance1() == 0; + return sc_ && sc_->Dance1() == 0; } catch (const std::exception& e) { LOG_ERROR("Dance1 failed: " + std::string(e.what())); return false; @@ -175,7 +159,7 @@ bool Controller::Dance1() { bool Controller::Dance2() { try { - return sportClient_ && sportClient_->Dance2() == 0; + return sc_ && sc_->Dance2() == 0; } catch (const std::exception& e) { LOG_ERROR("Dance2 failed: " + std::string(e.what())); return false; @@ -184,7 +168,7 @@ bool Controller::Dance2() { bool Controller::Hello() { try { - return sportClient_ && sportClient_->Hello() == 0; + return sc_ && sc_->Hello() == 0; } catch (const std::exception& e) { LOG_ERROR("Hello failed: " + std::string(e.what())); return false; @@ -196,7 +180,7 @@ bool Controller::Hello() { bool Controller::SwitchSet(bool enable) { try { - return obstacleClient_ && obstacleClient_->SwitchSet(enable) == 0; + return oac_ && oac_->SwitchSet(enable) == 0; } catch (const std::exception& e) { LOG_ERROR("Switch set failed: " + std::string(e.what())); return false; @@ -205,7 +189,7 @@ bool Controller::SwitchSet(bool enable) { bool Controller::SwitchGet(bool& enable) { try { - return obstacleClient_ && obstacleClient_->SwitchGet(enable) == 0; + return oac_ && oac_->SwitchGet(enable) == 0; } catch (const std::exception& e) { LOG_ERROR("Switch get failed: " + std::string(e.what())); return false; @@ -214,7 +198,7 @@ bool Controller::SwitchGet(bool& enable) { bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) { try { - return obstacleClient_ && obstacleClient_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0; + return oac_ && oac_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0; } catch (const std::exception& e) { LOG_ERROR("Use remote command from api failed: " + std::string(e.what())); return false; @@ -223,7 +207,7 @@ bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) { bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) { try { - return obstacleClient_ && obstacleClient_->MoveToAbsolutePosition(x, y, yaw) == 0; + return oac_ && oac_->MoveToAbsolutePosition(x, y, yaw) == 0; } catch (const std::exception& e) { LOG_ERROR("Move to absolute position failed: " + std::string(e.what())); return false; @@ -232,7 +216,7 @@ bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) { bool Controller::MoveToIncrementPosition(float x, float y, float yaw) { try { - return obstacleClient_ && obstacleClient_->MoveToIncrementPosition(x, y, yaw) == 0; + return oac_ && oac_->MoveToIncrementPosition(x, y, yaw) == 0; } catch (const std::exception& e) { LOG_ERROR("Move to increment position failed: " + std::string(e.what())); return false; diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index c0ec0af..47e4007 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -2,13 +2,22 @@ #include #include #include +#include namespace custom { CustomRobot::CustomRobot() : initialized_(false), running_(false) { - + // 创建并加载配置 + config_.loadDefaults(); + + try { + unitree::robot::ChannelFactory::Instance()->Init(0, config_.network_interface); + LOG_INFO("ChannelFactory initialized with interface: " + config_.network_interface); + } catch (const std::exception& e) { + LOG_ERROR("Failed to initialize ChannelFactory: " + std::string(e.what())); } +} CustomRobot::~CustomRobot() { LOG_INFO("Shutting down CustomRobot"); @@ -17,32 +26,30 @@ CustomRobot::~CustomRobot() { controller_.reset(); } - if (stateClient_) { - stateClient_.reset(); + if (rsc_) { + rsc_.reset(); + } + + try { + unitree::robot::ChannelFactory::Instance()->Release(); + LOG_INFO("ChannelFactory released"); + } catch (const std::exception& e) { + LOG_ERROR("Failed to release ChannelFactory: " + std::string(e.what())); } initialized_ = false; } bool CustomRobot::initialize() { - LOG_INFO("Initializing CustomRobot"); - - auto& config = Config::getInstance().getConfig(); - - controller_ = std::make_unique(config.network_interface); - if (!controller_->initialize()) { - LOG_ERROR("Failed to initialize robot controller"); - return false; - } - - // Initialize robot state client try { - stateClient_ = std::make_unique(); - stateClient_->SetTimeout(10.0f); - stateClient_->Init(); - LOG_INFO("RobotStateClient initialized successfully"); + controller_ = std::make_unique(); + controller_->initialize(); + + rsc_ = std::make_unique(); + rsc_->SetTimeout(3.0); + rsc_->Init(); } catch (const std::exception& e) { - LOG_ERROR("Failed to initialize RobotStateClient: " + std::string(e.what())); + LOG_ERROR("Failed to initialize : " + std::string(e.what())); return false; } @@ -63,6 +70,23 @@ bool CustomRobot::start() { return true; } + std::vector serviceList; + if (!GetServiceList(serviceList)) { + LOG_ERROR("Failed to get service list"); + return false; + } + + for (const auto& service : serviceList) { + LOG_INFO("Service - name: " + service.name + ", protect: " + std::to_string(service.protect) + ", status: " + std::to_string(service.status)); + } + + if (!SwitchService("sport_mode", false)) { + LOG_ERROR("Failed to stop sport_mode service"); + return false; + } + + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (!controller_->start()) { LOG_ERROR("Failed to start robot controller"); return false; @@ -73,13 +97,13 @@ bool CustomRobot::start() { } bool CustomRobot::GetServiceList(std::vector& serviceList) { - if (!initialized_ || !stateClient_) { + if (!initialized_ || !rsc_) { LOG_ERROR("Robot not initialized or stateClient is null"); return false; } try { - int32_t ret = stateClient_->ServiceList(serviceList); + int32_t ret = rsc_->ServiceList(serviceList); if (ret != 0) { LOG_ERROR("Failed to get service list, error code: " + std::to_string(ret)); return false; @@ -93,14 +117,14 @@ bool CustomRobot::GetServiceList(std::vector& } bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) { - if (!initialized_ || !stateClient_) { + if (!initialized_ || !rsc_) { LOG_ERROR("Robot not initialized or stateClient is null"); return false; } try { int32_t status; - int32_t ret = stateClient_->ServiceSwitch(serviceName, enable ? 1 : 0, status); + int32_t ret = rsc_->ServiceSwitch(serviceName, enable ? 1 : 0, status); if (ret != 0) { LOG_ERROR("Failed to switch service " + serviceName + ", error code: " + std::to_string(ret)); return false; @@ -114,13 +138,13 @@ bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) { } bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) { - if (!initialized_ || !stateClient_) { + if (!initialized_ || !rsc_) { LOG_ERROR("Robot not initialized or stateClient is null"); return false; } try { - int32_t ret = stateClient_->SetReportFreq(interval, duration); + int32_t ret = rsc_->SetReportFreq(interval, duration); if (ret != 0) { LOG_ERROR("Failed to set report frequency, error code: " + std::to_string(ret)); return false; @@ -133,4 +157,4 @@ bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) { } } -} +} // namespace custom diff --git a/src/main.cpp b/src/main.cpp index 4c5a08f..2a502d6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,7 +23,7 @@ int main(int argc, char** argv) { signal(SIGTERM, signalHandler); try { - g_robot = std::make_unique(""); + g_robot = std::make_unique(); if (!g_robot->initialize()) { LOG_ERROR("Failed to initialize robot");