diff --git a/CMakeLists.txt b/CMakeLists.txt index 5291d57..f2eacea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,9 +12,7 @@ find_package(Threads REQUIRED) # Find installed unitree_sdk2 find_package(unitree_sdk2 REQUIRED) -# Find MQTT library (Eclipse Paho) -pkg_check_modules(PAHO_MQTT REQUIRED libpaho-mqtt3c) -pkg_check_modules(PAHO_MQTTPP REQUIRED libpaho-mqttpp3) +# MQTT library is no longer required # Find JSON library find_package(nlohmann_json REQUIRED) @@ -27,7 +25,6 @@ include_directories( # Source files set(SOURCES src/main.cpp - src/mqtt.cpp src/controller.cpp src/custom_robot.cpp src/config.cpp @@ -40,18 +37,10 @@ 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} diff --git a/include/config.hpp b/include/config.hpp index 3f2f6d3..513b800 100644 --- a/include/config.hpp +++ b/include/config.hpp @@ -1,192 +1,207 @@ #pragma once #include -#include +#include -namespace custom { +namespace customConfig { -// Compile-time configuration flag -constexpr bool USE_COMPILE_TIME_CONFIG = true; +// Configuration template selector +enum class ConfigPreset { + Default, + HighPerformance, + Development, + Safety +}; -// Network settings -constexpr const char* NETWORK_INTERFACE = "eth0"; +// Motion gait enum +enum class Gait : int { + IDLE = 0, + TROT = 1, + TROT_RUNNING = 2 +}; -// MQTT settings -constexpr const char* MQTT_BROKER = "localhost"; +// Network configuration +constexpr std::string_view NETWORK_INTERFACE = "eth0"; + +// MQTT configuration +constexpr std::string_view 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"; +constexpr std::string_view MQTT_USERNAME = ""; +constexpr std::string_view MQTT_PASSWORD = ""; +constexpr std::string_view 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"; +// Topic configuration +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 settings +// Robot control configuration 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 +// 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 -// Motion settings +// Motion configuration constexpr double STAND_HEIGHT = 0.0; // relative height -constexpr int GAIT = 0; // 0:idle, 1:trot, 2:trot_running +constexpr Gait DEFAULT_GAIT = Gait::IDLE; -// Video settings +// Video configuration constexpr int VIDEO_WIDTH = 1280; constexpr int VIDEO_HEIGHT = 720; constexpr int VIDEO_FPS = 30; -constexpr const char* VIDEO_FORMAT = "h264"; +constexpr std::string_view VIDEO_FORMAT = "h264"; +constexpr bool VIDEO_ENABLED = true; -// Audio settings +// Audio configuration constexpr int AUDIO_SAMPLE_RATE = 16000; constexpr int AUDIO_CHANNELS = 1; -constexpr const char* AUDIO_FORMAT = "pcm"; +constexpr std::string_view AUDIO_FORMAT = "pcm"; +constexpr bool AUDIO_ENABLED = true; -// 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 +// Configuration presets using template specialization +template +struct ConfigParams; + +// 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; }; -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; +// 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 }; -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; +// 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; }; -// Runtime configuration structure - uses compile-time defaults -struct RobotConfig { +// 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 - std::string network_interface = NETWORK_INTERFACE; + static constexpr std::string_view 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; + 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 - 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; + 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 - double control_frequency = CONTROL_FREQUENCY; - double state_publish_frequency = STATE_PUBLISH_FREQUENCY; - bool enable_video = ENABLE_VIDEO; - bool enable_audio = ENABLE_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 - double max_linear_velocity = MAX_LINEAR_VELOCITY; - double max_angular_velocity = MAX_ANGULAR_VELOCITY; - double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT; + // 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 - double stand_height = STAND_HEIGHT; - double default_gait = GAIT; + static constexpr double stand_height = STAND_HEIGHT; + static constexpr Gait default_gait = DEFAULT_GAIT; // Video settings - int video_width = VIDEO_WIDTH; - int video_height = VIDEO_HEIGHT; - int video_fps = VIDEO_FPS; - std::string video_format = VIDEO_FORMAT; + 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 - int audio_sample_rate = AUDIO_SAMPLE_RATE; - int audio_channels = AUDIO_CHANNELS; - std::string audio_format = AUDIO_FORMAT; + 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); + } }; -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; +// Type alias for current configuration +using RobotConfig = CompileTimeConfig; -private: - Config(); - ~Config() = default; - Config(const Config&) = delete; - Config& operator=(const Config&) = delete; - - RobotConfig config_; -}; +// 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; +} -} // namespace custom +// 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 diff --git a/include/controller.hpp b/include/controller.hpp index 36c4711..fa7fb0b 100644 --- a/include/controller.hpp +++ b/include/controller.hpp @@ -1,11 +1,7 @@ #pragma once #include -#include #include -#include -#include -#include #include #include @@ -14,220 +10,49 @@ #include #include #include -#include 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; - using ErrorCallback = std::function; - 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); + // Sport + bool StandUp(); + bool StandDown(); + bool Sit(); + bool Lie(); + bool Damp(); + bool RecoveryStand(); + bool StopMove(); + bool BalanceStand(); + bool Dance1(); + bool Dance2(); + bool Hello(); + + // Obstacle + bool SwitchSet(bool enable); + bool SwitchGet(bool& enable); + bool UseRemoteCommandFromApi(bool isRemoteCommandsFromApi); + bool MoveToAbsolutePosition(float x, float y, float yaw); + bool MoveToIncrementPosition(float x, float y, float yaw); - // 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 sportClient_; std::unique_ptr obstacleClient_; - std::unique_ptr videoClient_; - std::unique_ptr vuiClient_; - // State subscribers - std::unique_ptr> sportStateSubscriber_; - std::unique_ptr> lowStateSubscriber_; - - // Current state - mutable std::mutex stateMutex_; - RobotState currentState_; - - // Command queue - std::queue commandQueue_; - std::mutex commandMutex_; - std::condition_variable commandCondition_; - - // Control threads - std::thread controlThread_; - std::thread stateThread_; std::atomic running_; std::atomic initialized_; - - // Callbacks - StateCallback stateCallback_; - ErrorCallback errorCallback_; - - // Safety - std::chrono::steady_clock::time_point lastCommandTime_; - std::atomic emergencyStopActive_; - - // Network std::string networkInterface_; - - // Timing - std::chrono::milliseconds controlPeriod_; - std::chrono::milliseconds statePeriod_; }; } // namespace custom diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index 959f257..760fbdc 100644 --- a/include/custom_robot.hpp +++ b/include/custom_robot.hpp @@ -1,123 +1,34 @@ #pragma once #include "controller.hpp" -#include "mqtt.hpp" #include "config.hpp" #include "logger.hpp" #include -#include #include -#include -#include +#include namespace custom { class CustomRobot { public: - explicit CustomRobot(const std::string& configFile = ""); + explicit CustomRobot(); ~CustomRobot(); bool initialize(); - void shutdown(); - bool start(); - bool stop(); - bool isRunning() const { return running_; } - // Main run loop - void run(); - void runAsync(); - void waitForShutdown(); + // Robot state methods + bool GetServiceList(std::vector& serviceList); + bool SwitchService(const std::string& serviceName, bool enable); + bool SetReportFreq(int32_t interval, int32_t duration); 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); + std::unique_ptr controller_; + std::unique_ptr stateClient_; - // 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 robotController_; - std::unique_ptr mqttClient_; - - // Configuration - std::string configFile_; - - // State std::atomic running_; std::atomic initialized_; - std::atomic mqttConnected_; - - // Threads - std::thread mainThread_; - std::thread safetyThread_; - std::vector 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 pendingRequests_; - - // Error handling - std::mutex errorMutex_; - std::queue errorQueue_; - void processErrorQueue(); }; } // namespace custom diff --git a/src/config.cpp b/src/config.cpp index 42f1209..3d502ec 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -3,7 +3,7 @@ #include #include -namespace custom { +namespace customConfig { Config::Config() { // Load default configuration @@ -16,8 +16,6 @@ Config& Config::getInstance() { } void Config::loadDefaults() { - using namespace config; - config_.network_interface = NETWORK_INTERFACE; config_.mqtt_broker = MQTT_BROKER; config_.mqtt_port = MQTT_PORT; @@ -33,15 +31,15 @@ void Config::loadDefaults() { config_.control_frequency = CONTROL_FREQUENCY; config_.state_publish_frequency = STATE_PUBLISH_FREQUENCY; - config_.enable_video = ENABLE_VIDEO; - config_.enable_audio = ENABLE_AUDIO; + 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 = GAIT; + config_.default_gait = static_cast(DEFAULT_GAIT); config_.video_width = VIDEO_WIDTH; config_.video_height = VIDEO_HEIGHT; @@ -55,46 +53,48 @@ void Config::loadDefaults() { 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; + 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(); - 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; + 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(); - 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; + 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) { - // If compile-time config is enabled, skip JSON loading - if (config::USE_COMPILE_TIME_CONFIG) { - loadDefaults(); - return true; - } + // 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"); - setDefaults(); + loadDefaults(); return false; } @@ -175,9 +175,12 @@ bool Config::loadConfig(const std::string& configFile) { } catch (const std::exception& e) { LOG_ERROR("Failed to load config: " + std::string(e.what())); - setDefaults(); + loadDefaults(); return false; } + */ + + return true; } bool Config::saveConfig(const std::string& configFile) { @@ -253,64 +256,64 @@ bool Config::validateConfig() { // 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; + 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 = config::STATE_PUBLISH_FREQUENCY; + 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 = config::MAX_LINEAR_VELOCITY; + 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 = config::MAX_ANGULAR_VELOCITY; + 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 = config::EMERGENCY_STOP_TIMEOUT; + 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 = config::VIDEO_WIDTH; - config_.video_height = config::VIDEO_HEIGHT; + 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 = config::VIDEO_FPS; + 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 = config::AUDIO_SAMPLE_RATE; + 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 = config::AUDIO_CHANNELS; + config_.audio_channels = AUDIO_CHANNELS; valid = false; } return valid; } -} // namespace custom +} // namespace customConfig diff --git a/src/controller.cpp b/src/controller.cpp index 6fbea62..a523707 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -7,63 +7,34 @@ 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(1000.0 / config.control_frequency)); - statePeriod_ = std::chrono::milliseconds(static_cast(1000.0 / config.state_publish_frequency)); + : networkInterface_(networkInterface), running_(false), initialized_(false) { } Controller::~Controller() { - shutdown(); + if (running_) { + stop(); + } + + LOG_INFO("Shutting down robot controller"); + sportClient_.reset(); + obstacleClient_.reset(); + initialized_ = false; } 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(); sportClient_->SetTimeout(10.0f); sportClient_->Init(); - // Initialize obstacle avoidance client obstacleClient_ = std::make_unique(); + obstacleClient_->SetTimeout(3.0f); obstacleClient_->Init(); - // Initialize video client - videoClient_ = std::make_unique(); - videoClient_->Init(); - - // Initialize VUI client - vuiClient_ = std::make_unique(); - vuiClient_->Init(); - - // Initialize state subscribers - sportStateSubscriber_.reset( - new unitree::robot::ChannelSubscriber("rt/sportmodestate") - ); - sportStateSubscriber_->InitChannel( - std::bind(&Controller::updateSportModeState, this, std::placeholders::_1), 1 - ); - - lowStateSubscriber_.reset( - new unitree::robot::ChannelSubscriber("rt/lowstate") - ); - lowStateSubscriber_->InitChannel( - std::bind(&Controller::updateLowLevelState, this, std::placeholders::_1), 1 - ); - - // Initialize robot state - { - std::lock_guard lock(stateMutex_); - currentState_ = RobotState{}; - currentState_.timestamp = std::chrono::steady_clock::now(); - } initialized_ = true; LOG_INFO("Robot controller initialized successfully"); @@ -75,26 +46,6 @@ bool Controller::initialize() { } } -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"); @@ -109,12 +60,6 @@ bool Controller::start() { 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; @@ -127,100 +72,16 @@ bool Controller::stop() { 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 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 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() { +bool Controller::StandUp() { LOG_INFO("Standing up"); try { return sportClient_ && sportClient_->StandUp() == 0; @@ -230,7 +91,7 @@ bool Controller::standUp() { } } -bool Controller::standDown() { +bool Controller::StandDown() { LOG_INFO("Standing down"); try { return sportClient_ && sportClient_->StandDown() == 0; @@ -240,7 +101,7 @@ bool Controller::standDown() { } } -bool Controller::sit() { +bool Controller::Sit() { LOG_INFO("Sitting"); try { return sportClient_ && sportClient_->Sit() == 0; @@ -250,7 +111,7 @@ bool Controller::sit() { } } -bool Controller::lie() { +bool Controller::Lie() { LOG_INFO("Lying down"); try { return sportClient_ && sportClient_->StandDown() == 0; @@ -260,7 +121,7 @@ bool Controller::lie() { } } -bool Controller::damp() { +bool Controller::Damp() { LOG_INFO("Damping"); try { return sportClient_ && sportClient_->Damp() == 0; @@ -270,57 +131,31 @@ bool Controller::damp() { } } -// 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) { +bool Controller::RecoveryStand() { + LOG_INFO("Executing recovery stand"); + try { if (sportClient_) { - sportClient_->Euler(roll, pitch, yaw); - sportClient_->BodyHeight(height); - return true; + int32_t result = sportClient_->RecoveryStand(); + return result == 0; } return false; } catch (const std::exception& e) { - LOG_ERROR("Set body pose failed: " + std::string(e.what())); + LOG_ERROR("Recovery stand failed: " + std::string(e.what())); return false; } } -bool Controller::balanceStand() { +bool Controller::StopMove() { + try { + return sportClient_ && sportClient_->StopMove() == 0; + } catch (const std::exception& e) { + LOG_ERROR("Stop move failed: " + std::string(e.what())); + return false; + } +} + +bool Controller::BalanceStand() { try { return sportClient_ && sportClient_->BalanceStand() == 0; } catch (const std::exception& e) { @@ -329,8 +164,7 @@ bool Controller::balanceStand() { } } -// Special actions -bool Controller::dance1() { +bool Controller::Dance1() { try { return sportClient_ && sportClient_->Dance1() == 0; } catch (const std::exception& e) { @@ -339,7 +173,7 @@ bool Controller::dance1() { } } -bool Controller::dance2() { +bool Controller::Dance2() { try { return sportClient_ && sportClient_->Dance2() == 0; } catch (const std::exception& e) { @@ -348,7 +182,7 @@ bool Controller::dance2() { } } -bool Controller::hello() { +bool Controller::Hello() { try { return sportClient_ && sportClient_->Hello() == 0; } catch (const std::exception& e) { @@ -357,219 +191,52 @@ bool Controller::hello() { } } -// 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 lock(commandMutex_); - - while (!commandQueue_.empty()) { - auto command = commandQueue_.front(); - commandQueue_.pop(); - lock.unlock(); - - executeMotionCommand(command); - - lock.lock(); + +bool Controller::SwitchSet(bool enable) { + try { + return obstacleClient_ && obstacleClient_->SwitchSet(enable) == 0; + } catch (const std::exception& e) { + LOG_ERROR("Switch set failed: " + std::string(e.what())); + return false; } } -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::SwitchGet(bool& enable) { + try { + return obstacleClient_ && obstacleClient_->SwitchGet(enable) == 0; + } catch (const std::exception& e) { + LOG_ERROR("Switch get failed: " + std::string(e.what())); + return false; } } -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(message); - - std::lock_guard 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(message); - - std::lock_guard 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( - 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 lock(stateMutex_); - if (currentState_.battery_percentage < 10) { - currentState_.low_battery = true; - if (!emergencyStopActive_) { - LOG_WARN("Low battery detected, activating emergency stop"); - emergencyStop(); - } - } +bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) { + try { + return obstacleClient_ && obstacleClient_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0; + } catch (const std::exception& e) { + LOG_ERROR("Use remote command from api failed: " + std::string(e.what())); + return false; } } -void Controller::handleError(const std::string& error) { - if (errorCallback_) { - errorCallback_(error); +bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) { + try { + return obstacleClient_ && obstacleClient_->MoveToAbsolutePosition(x, y, yaw) == 0; + } catch (const std::exception& e) { + LOG_ERROR("Move to absolute position failed: " + std::string(e.what())); + return false; } } -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; +bool Controller::MoveToIncrementPosition(float x, float y, float yaw) { + try { + return obstacleClient_ && obstacleClient_->MoveToIncrementPosition(x, y, yaw) == 0; + } catch (const std::exception& e) { + LOG_ERROR("Move to increment position failed: " + std::string(e.what())); + return false; + } } -} // namespace custom +} // namespace custom \ No newline at end of file diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index cb0c940..c0ec0af 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -1,71 +1,48 @@ #include "custom_robot.hpp" +#include #include -#include +#include 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() + : initialized_(false), running_(false) { + + } CustomRobot::~CustomRobot() { - shutdown(); + LOG_INFO("Shutting down CustomRobot"); + if (controller_) { + controller_->shutdown(); + controller_.reset(); + } + + if (stateClient_) { + stateClient_.reset(); + } + + initialized_ = false; } 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(config.network_interface); - if (!robotController_->initialize()) { + controller_ = std::make_unique(config.network_interface); + if (!controller_->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( - 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"); + // Initialize robot state client + try { + stateClient_ = std::make_unique(); + stateClient_->SetTimeout(10.0f); + stateClient_->Init(); + LOG_INFO("RobotStateClient initialized successfully"); + } catch (const std::exception& e) { + LOG_ERROR("Failed to initialize RobotStateClient: " + std::string(e.what())); return false; } @@ -74,24 +51,6 @@ bool CustomRobot::initialize() { 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_) { @@ -104,579 +63,74 @@ bool CustomRobot::start() { return true; } - LOG_INFO("Starting CustomRobot"); - - // Start robot controller - if (!robotController_->start()) { + if (!controller_->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_) { +bool CustomRobot::GetServiceList(std::vector& serviceList) { + if (!initialized_ || !stateClient_) { + LOG_ERROR("Robot not initialized or stateClient is null"); + return false; + } + + try { + int32_t ret = stateClient_->ServiceList(serviceList); + if (ret != 0) { + LOG_ERROR("Failed to get service list, error code: " + std::to_string(ret)); + return false; + } + LOG_INFO("Successfully retrieved service list with " + std::to_string(serviceList.size()) + " services"); 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::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())); + LOG_ERROR("Exception in getServiceList: " + std::string(e.what())); + return false; } } -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_) { +bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) { + if (!initialized_ || !stateClient_) { + LOG_ERROR("Robot not initialized or stateClient is null"); return false; } - MotionCommand motionCmd = jsonToMotionCommand(cmd); - return robotController_->executeCommand(motionCmd); + try { + int32_t status; + int32_t ret = stateClient_->ServiceSwitch(serviceName, enable ? 1 : 0, status); + if (ret != 0) { + LOG_ERROR("Failed to switch service " + serviceName + ", error code: " + std::to_string(ret)); + return false; + } + LOG_INFO("Successfully switched service " + serviceName + " to " + (enable ? "enabled" : "disabled")); + return true; + } catch (const std::exception& e) { + LOG_ERROR("Exception in switchService: " + std::string(e.what())); + return false; + } } -bool CustomRobot::processSpecialAction(const nlohmann::json& cmd) { - if (!robotController_ || !cmd.contains("action")) { +bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) { + if (!initialized_ || !stateClient_) { + LOG_ERROR("Robot not initialized or stateClient is null"); 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")) { + try { + int32_t ret = stateClient_->SetReportFreq(interval, duration); + if (ret != 0) { + LOG_ERROR("Failed to set report frequency, error code: " + std::to_string(ret)); + return false; + } + LOG_INFO("Successfully set report frequency: interval=" + std::to_string(interval) + ", duration=" + std::to_string(duration)); + return true; + } catch (const std::exception& e) { + LOG_ERROR("Exception in setReportFreq: " + std::string(e.what())); 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 lock(stateCacheMutex_); - lastKnownState_ = state; - } - - // Publish state if enough time has passed - auto now = std::chrono::steady_clock::now(); - auto timeSinceLastPublish = std::chrono::duration(now - lastStatePublish_).count(); - - if (timeSinceLastPublish >= statePublishInterval_) { - publishState(); - lastStatePublish_ = now; - } -} - -void CustomRobot::handleRobotError(const std::string& error) { - { - std::lock_guard lock(errorMutex_); - errorQueue_.push(error); - } - - stats_.errorsOccurred++; -} - -void CustomRobot::publishState() { - if (!mqttClient_ || !mqttClient_->isConnected()) { - return; - } - - try { - RobotState state; - { - std::lock_guard 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::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::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( - 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(state.motor_positions, state.motor_positions + 12); - json["motors"]["velocities"] = std::vector(state.motor_velocities, state.motor_velocities + 12); - json["motors"]["torques"] = std::vector(state.motor_torques, state.motor_torques + 12); - json["motors"]["temperatures"] = std::vector(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(now - stats_.startTime).count(); - status["uptime_seconds"] = uptime; - - status["timestamp"] = std::chrono::duration_cast( - 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(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(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 lock(errorMutex_); - - while (!errorQueue_.empty()) { - std::string error = errorQueue_.front(); - errorQueue_.pop(); - - publishError(error); - } -} - -} // namespace custom +} diff --git a/src/main.cpp b/src/main.cpp index 27b0d9f..4c5a08f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,24 +11,18 @@ std::unique_ptr 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(""); if (!g_robot->initialize()) { @@ -37,11 +31,7 @@ int main(int argc, char** argv) { } LOG_INFO("Robot initialized successfully"); - LOG_INFO("Press Ctrl+C to stop the robot"); - - // Run the robot - g_robot->run(); - + g_robot->start(); } catch (const std::exception& e) { LOG_ERROR("Exception: " + std::string(e.what())); return 1;