Remove MQTT dependency and refactor configuration structure. Updated CMakeLists.txt to eliminate MQTT library checks and adjusted config.hpp to use string_view for configuration parameters. Simplified controller and custom_robot classes by removing MQTT-related code and enhancing robot state management. Introduced service management methods in CustomRobot for better state handling.

CMakeLists.txt

@@ -12,9 +12,7 @@ find_package(Threads REQUIRED)
 # Find installed unitree_sdk2
 find_package(unitree_sdk2 REQUIRED)
 
-# Find MQTT library (Eclipse Paho)
+# MQTT library is no longer required
-pkg_check_modules(PAHO_MQTT REQUIRED libpaho-mqtt3c)
-pkg_check_modules(PAHO_MQTTPP REQUIRED libpaho-mqttpp3)
 
 # 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}

include/config.hpp

@@ -1,192 +1,207 @@
 #pragma once
 
 #include <string>
-#include <nlohmann/json.hpp>
+#include <string_view>
 
-namespace custom {
+namespace customConfig {
 
-// Compile-time configuration flag
+// Configuration template selector
-constexpr bool USE_COMPILE_TIME_CONFIG = true;
+enum class ConfigPreset {
+    Default,
+    HighPerformance,
+    Development,
+    Safety
+};
 
-// Network settings
+// Motion gait enum
-constexpr const char* NETWORK_INTERFACE = "eth0";
+enum class Gait : int {
+    IDLE = 0,
+    TROT = 1,
+    TROT_RUNNING = 2
+};
 
-// MQTT settings
+// Network configuration
-constexpr const char* MQTT_BROKER = "localhost";
+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 std::string_view MQTT_USERNAME = "";
-constexpr const char* MQTT_PASSWORD = "";
+constexpr std::string_view MQTT_PASSWORD = "";
-constexpr const char* MQTT_CLIENT_ID = "unitree_go2_client";
+constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client";
 
-// Topic settings
+// Topic configuration
-constexpr const char* TOPIC_PREFIX = "unitree/go2";
+constexpr std::string_view TOPIC_PREFIX = "unitree/go2";
-constexpr const char* TOPIC_CMD = "cmd";
+constexpr std::string_view TOPIC_CMD = "cmd";
-constexpr const char* TOPIC_STATE = "state";
+constexpr std::string_view TOPIC_STATE = "state";
-constexpr const char* TOPIC_VIDEO = "video";
+constexpr std::string_view TOPIC_VIDEO = "video";
-constexpr const char* TOPIC_AUDIO = "audio";
+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
+// Configuration presets using template specialization
-struct HighPerformancePreset {
+template<ConfigPreset P>
-    static constexpr double CONTROL_FREQUENCY = 500.0;
+struct ConfigParams;
-    static constexpr double STATE_PUBLISH_FREQUENCY = 100.0;
+
-    static constexpr double MAX_LINEAR_VELOCITY = 2.5;
+// Default preset
-    static constexpr double MAX_ANGULAR_VELOCITY = 3.0;
+template<>
-    static constexpr bool ENABLE_VIDEO = false; // Disable for performance
+struct ConfigParams<ConfigPreset::Default> {
-    static constexpr bool ENABLE_AUDIO = false; // Disable for performance
+    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 {
+// High performance preset
-    static constexpr double CONTROL_FREQUENCY = 100.0;
+template<>
-    static constexpr double STATE_PUBLISH_FREQUENCY = 20.0;
+struct ConfigParams<ConfigPreset::HighPerformance> {
-    static constexpr double MAX_LINEAR_VELOCITY = 0.8;
+    static constexpr double control_frequency = 500.0;
-    static constexpr double MAX_ANGULAR_VELOCITY = 1.0;
+    static constexpr double state_publish_frequency = 100.0;
-    static constexpr bool ENABLE_VIDEO = true;
+    static constexpr double max_linear_velocity = 2.5;
-    static constexpr bool ENABLE_AUDIO = true;
+    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 {
+// Development preset
-    static constexpr double CONTROL_FREQUENCY = 50.0;
+template<>
-    static constexpr double STATE_PUBLISH_FREQUENCY = 10.0;
+struct ConfigParams<ConfigPreset::Development> {
-    static constexpr double MAX_LINEAR_VELOCITY = 0.5;
+    static constexpr double control_frequency = 100.0;
-    static constexpr double MAX_ANGULAR_VELOCITY = 0.5;
+    static constexpr double state_publish_frequency = 20.0;
-    static constexpr double EMERGENCY_STOP_TIMEOUT = 2.0;
+    static constexpr double max_linear_velocity = 0.8;
-    static constexpr bool ENABLE_VIDEO = true;
+    static constexpr double max_angular_velocity = 1.0;
-    static constexpr bool ENABLE_AUDIO = true;
+    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
+// Safety preset
-struct RobotConfig {
+template<>
+struct ConfigParams<ConfigPreset::Safety> {
+    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<ConfigPreset P = ACTIVE_PRESET>
+struct CompileTimeConfig {
+    // Use selected preset parameters
+    using params = ConfigParams<P>;
+    
     // 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;
+    static constexpr std::string_view mqtt_broker = MQTT_BROKER;
-    int mqtt_port = MQTT_PORT;
+    static constexpr int mqtt_port = MQTT_PORT;
-    std::string mqtt_username = MQTT_USERNAME;
+    static constexpr std::string_view mqtt_username = MQTT_USERNAME;
-    std::string mqtt_password = MQTT_PASSWORD;
+    static constexpr std::string_view mqtt_password = MQTT_PASSWORD;
-    std::string mqtt_client_id = MQTT_CLIENT_ID;
+    static constexpr std::string_view mqtt_client_id = MQTT_CLIENT_ID;
     
     // Topics
-    std::string topic_prefix = TOPIC_PREFIX;
+    static constexpr std::string_view topic_prefix = TOPIC_PREFIX;
-    std::string cmd_topic = TOPIC_CMD;
+    static constexpr std::string_view cmd_topic = TOPIC_CMD;
-    std::string state_topic = TOPIC_STATE;
+    static constexpr std::string_view state_topic = TOPIC_STATE;
-    std::string video_topic = TOPIC_VIDEO;
+    static constexpr std::string_view video_topic = TOPIC_VIDEO;
-    std::string audio_topic = TOPIC_AUDIO;
+    static constexpr std::string_view audio_topic = TOPIC_AUDIO;
     
-    // Robot settings
+    // Robot settings (from preset)
-    double control_frequency = CONTROL_FREQUENCY;
+    static constexpr double control_frequency = params::control_frequency;
-    double state_publish_frequency = STATE_PUBLISH_FREQUENCY;
+    static constexpr double state_publish_frequency = params::state_publish_frequency;
-    bool enable_video = ENABLE_VIDEO;
+    static constexpr bool enable_video = params::video_enabled;
-    bool enable_audio = ENABLE_AUDIO;
+    static constexpr bool enable_audio = params::audio_enabled;
     
-    // Safety settings
+    // Safety settings (from preset)
-    double max_linear_velocity = MAX_LINEAR_VELOCITY;
+    static constexpr double max_linear_velocity = params::max_linear_velocity;
-    double max_angular_velocity = MAX_ANGULAR_VELOCITY;
+    static constexpr double max_angular_velocity = params::max_angular_velocity;
-    double emergency_stop_timeout = EMERGENCY_STOP_TIMEOUT;
+    static constexpr double emergency_stop_timeout = params::emergency_stop_timeout;
     
     // Motion settings
-    double stand_height = STAND_HEIGHT;
+    static constexpr double stand_height = STAND_HEIGHT;
-    double default_gait = GAIT;
+    static constexpr Gait default_gait = DEFAULT_GAIT;
     
     // Video settings
-    int video_width = VIDEO_WIDTH;
+    static constexpr int video_width = VIDEO_WIDTH;
-    int video_height = VIDEO_HEIGHT;
+    static constexpr int video_height = VIDEO_HEIGHT;
-    int video_fps = VIDEO_FPS;
+    static constexpr int video_fps = VIDEO_FPS;
-    std::string video_format = VIDEO_FORMAT;
+    static constexpr std::string_view video_format = VIDEO_FORMAT;
     
     // Audio settings
-    int audio_sample_rate = AUDIO_SAMPLE_RATE;
+    static constexpr int audio_sample_rate = AUDIO_SAMPLE_RATE;
-    int audio_channels = AUDIO_CHANNELS;
+    static constexpr int audio_channels = AUDIO_CHANNELS;
-    std::string audio_format = AUDIO_FORMAT;
+    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 {
+// Type alias for current configuration
-public:
+using RobotConfig = CompileTimeConfig<ACTIVE_PRESET>;
-    static Config& getInstance();
 
-    /**
+// Configuration validation utilities
-     * Load configuration from JSON file
+template<ConfigPreset P>
-     */
+constexpr bool isConfigValid() {
-    bool loadConfig(const std::string& configPath);
+    using config = ConfigParams<P>;
+    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
-     * Save current configuration to JSON file
+static_assert(isConfigValid<ACTIVE_PRESET>(), "Invalid configuration parameters");
-     */
-    bool saveConfig(const std::string& configPath);
 
-    /**
+// Configuration preset name utilities
-     * Load configuration from compile-time defaults
+template<ConfigPreset P>
-     */
+constexpr const char* getPresetName() {
-    void loadDefaults();
+    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() {
-     * Load high-performance preset
+    return getPresetName<ACTIVE_PRESET>();
-     */
+}
-    void loadHighPerformancePreset();
 
-    /**
+} // namespace customConfig
-     * Load development preset
-     */
-    void loadDevelopmentPreset();
-    
-    /**
-     * Load safety preset
-     */
-    void loadSafetyPreset();
-    
-    /**
-     * Validate configuration values
-     */
-    bool validateConfig();
-    
-    /**
-     * Get current configuration
-     */
-    const RobotConfig& getConfig() const { return config_; }
-    
-    /**
-     * Get mutable reference to configuration
-     */
-    RobotConfig& getConfig() { return config_; }
-    
-    /**
-     * Convenience getters
-     */
-    std::string getFullTopic(const std::string& topic) const;
-
-private:
-    Config();
-    ~Config() = default;
-    Config(const Config&) = delete;
-    Config& operator=(const Config&) = delete;
-    
-    RobotConfig config_;
-};
-
-} // namespace custom

include/controller.hpp

@@ -1,11 +1,7 @@
 #pragma once
 
 #include <memory>
-#include <thread>
 #include <atomic>
-#include <mutex>
-#include <condition_variable>
-#include <chrono>
 #include <nlohmann/json.hpp>
 
 #include <unitree/robot/go2/sport/sport_client.hpp>
@@ -14,220 +10,49 @@
 #include <unitree/robot/go2/vui/vui_client.hpp>
 #include <unitree/robot/channel/channel_subscriber.hpp>
 #include <unitree/idl/go2/SportModeState_.hpp>
-#include <unitree/idl/go2/LowState_.hpp>
 
 namespace custom {
 
-enum class RobotMode {
-    IDLE,
-    SPORT,
-    LOW_LEVEL,
-    RECOVERY
-};
 
-enum class MotionState {
-    STOPPED,
-    STANDING,
-    WALKING,
-    RUNNING,
-    SITTING,
-    LYING,
-    DANCING,
-    SPECIAL_MOTION
-};
-
-struct RobotState {
-    RobotMode mode = RobotMode::IDLE;
-    MotionState motion_state = MotionState::STOPPED;
-    
-    // Position and orientation
-    double position[3] = {0.0, 0.0, 0.0}; // x, y, z
-    double orientation[3] = {0.0, 0.0, 0.0}; // roll, pitch, yaw
-    double velocity[3] = {0.0, 0.0, 0.0}; // vx, vy, vyaw
-    
-    // IMU data
-    double imu_acc[3] = {0.0, 0.0, 0.0};
-    double imu_gyro[3] = {0.0, 0.0, 0.0};
-    double imu_quat[4] = {1.0, 0.0, 0.0, 0.0};
-    
-    // Battery and system
-    double battery_voltage = 0.0;
-    double battery_current = 0.0;
-    int battery_percentage = 0;
-    double temperature = 0.0;
-    
-    // Motor states (20 motors)
-    double motor_positions[20] = {0};
-    double motor_velocities[20] = {0};
-    double motor_torques[20] = {0};
-    double motor_temperatures[20] = {0};
-    
-    // Status flags
-    bool is_connected = false;
-    bool emergency_stop = false;
-    bool low_battery = false;
-    bool overheated = false;
-    
-    std::chrono::steady_clock::time_point timestamp;
-};
-
-struct MotionCommand {
-    enum Type {
-        VELOCITY,
-        POSITION,
-        SPECIAL_ACTION,
-        GAIT_CHANGE,
-        BODY_POSE
-    } type;
-    
-    // Velocity command
-    double linear_velocity[3] = {0.0, 0.0, 0.0}; // vx, vy, vz
-    double angular_velocity[3] = {0.0, 0.0, 0.0}; // wx, wy, wz
-    
-    // Position command
-    double target_position[3] = {0.0, 0.0, 0.0};
-    double target_orientation[3] = {0.0, 0.0, 0.0};
-    
-    // Body pose
-    double body_height = 0.0;
-    double body_roll = 0.0;
-    double body_pitch = 0.0;
-    double body_yaw = 0.0;
-    
-    // Special actions
-    std::string action_name;
-    nlohmann::json action_params;
-    
-    // Gait parameters
-    int gait_type = 0;
-    double step_height = 0.0;
-    
-    std::chrono::steady_clock::time_point timestamp;
-    double duration = 0.0; // command duration in seconds
-};
 
 class Controller {
 public:
-    using StateCallback = std::function<void(const RobotState&)>;
-    using ErrorCallback = std::function<void(const std::string&)>;
-
     explicit Controller(const std::string& networkInterface);
     ~Controller();
 
     bool initialize();
-    void shutdown();
-
-    // State management
     bool start();
     bool stop();
     bool isRunning() const { return running_; }
     
-    RobotState getCurrentState() const;
+    // Sport
-    void setStateCallback(StateCallback callback);
+    bool StandUp();
-    void setErrorCallback(ErrorCallback callback);
+    bool StandDown();
+    bool Sit();
+    bool Lie();
+    bool Damp();
+    bool RecoveryStand();
+    bool StopMove();
+    bool BalanceStand();
+    bool Dance1();
+    bool Dance2();
+    bool Hello();
     
-    // Motion control
+    // Obstacle 
-    bool executeCommand(const MotionCommand& command);
+    bool SwitchSet(bool enable);
-    bool emergencyStop();
+    bool SwitchGet(bool& enable);
-    bool recoveryStand();
+    bool UseRemoteCommandFromApi(bool isRemoteCommandsFromApi);
+    bool MoveToAbsolutePosition(float x, float y, float yaw);
+    bool MoveToIncrementPosition(float x, float y, float yaw);
 
-    // Basic motions
-    bool standUp();
-    bool standDown();
-    bool sit();
-    bool lie();
-    bool damp();
-    
-    // Movement
-    bool move(double vx, double vy, double vyaw);
-    bool moveToPosition(double x, double y, double yaw);
-    bool stop();
-    
-    // Body control
-    bool setBodyPose(double roll, double pitch, double yaw, double height);
-    bool balanceStand();
-    
-    // Gait control
-    bool switchGait(int gaitType);
-    bool setSpeedLevel(int level);
-    
-    // Special actions
-    bool performAction(const std::string& actionName, const nlohmann::json& params = {});
-    
-    // Dance and tricks
-    bool dance1();
-    bool dance2();
-    bool frontFlip();
-    bool backFlip();
-    bool hello();
-    bool stretch();
-    
-    // Obstacle avoidance
-    bool enableObstacleAvoidance(bool enable);
-    bool isObstacleAvoidanceEnabled();
-    
-    // Video control
-    bool startVideo();
-    bool stopVideo();
-    bool isVideoActive();
-    
-    // Audio control
-    bool playAudio(const std::string& text);
-    bool stopAudio();
 
 private:
-    void controlLoop();
-    void stateUpdateLoop();
-    void processCommands();
-    
-    void updateSportModeState(const void* message);
-    void updateLowLevelState(const void* message);
-    
-    bool validateCommand(const MotionCommand& command);
-    void executeMotionCommand(const MotionCommand& command);
-    
-    void handleError(const std::string& error);
-    void checkSafetyLimits();
-    
-    // SDK clients
     std::unique_ptr<unitree::robot::go2::SportClient> sportClient_;
     std::unique_ptr<unitree::robot::go2::ObstaclesAvoidClient> obstacleClient_;
-    std::unique_ptr<unitree::robot::go2::VideoClient> videoClient_;
-    std::unique_ptr<unitree::robot::go2::VuiClient> vuiClient_;
     
-    // State subscribers
-    std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>> sportStateSubscriber_;
-    std::unique_ptr<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>> lowStateSubscriber_;
-    
-    // Current state
-    mutable std::mutex stateMutex_;
-    RobotState currentState_;
-    
-    // Command queue
-    std::queue<MotionCommand> commandQueue_;
-    std::mutex commandMutex_;
-    std::condition_variable commandCondition_;
-    
-    // Control threads
-    std::thread controlThread_;
-    std::thread stateThread_;
     std::atomic<bool> running_;
     std::atomic<bool> initialized_;
-    
-    // Callbacks
-    StateCallback stateCallback_;
-    ErrorCallback errorCallback_;
-    
-    // Safety
-    std::chrono::steady_clock::time_point lastCommandTime_;
-    std::atomic<bool> emergencyStopActive_;
-    
-    // Network
     std::string networkInterface_;
-    
-    // Timing
-    std::chrono::milliseconds controlPeriod_;
-    std::chrono::milliseconds statePeriod_;
 };
 
 } // namespace custom

include/custom_robot.hpp

@@ -1,123 +1,34 @@
 #pragma once
 
 #include "controller.hpp"
-#include "mqtt.hpp"
 #include "config.hpp"
 #include "logger.hpp"
 
 #include <memory>
-#include <thread>
 #include <atomic>
-#include <chrono>
+#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
-#include <nlohmann/json.hpp>
 
 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
+    // Robot state methods
-    void run();
+    bool GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList);
-    void runAsync();
+    bool SwitchService(const std::string& serviceName, bool enable);
-    void waitForShutdown();
+    bool SetReportFreq(int32_t interval, int32_t duration);
 
 private:
-    // MQTT message handlers
+    std::unique_ptr<Controller> controller_;
-    void handleMqttMessage(const std::string& topic, const std::string& payload);
+    std::unique_ptr<unitree::robot::go2::RobotStateClient> stateClient_;
-    void handleCommandMessage(const nlohmann::json& command);
-    void handleConfigMessage(const nlohmann::json& config);
-    void handleControlMessage(const nlohmann::json& control);
     
-    // Robot state handlers
-    void handleRobotState(const RobotState& state);
-    void handleRobotError(const std::string& error);
-    
-    // Publishing functions
-    void publishState();
-    void publishHeartbeat();
-    void publishError(const std::string& error);
-    void publishResponse(const std::string& requestId, bool success, const std::string& message = "");
-    
-    // Command processing
-    bool processMotionCommand(const nlohmann::json& cmd);
-    bool processSpecialAction(const nlohmann::json& cmd);
-    bool processSystemCommand(const nlohmann::json& cmd);
-    bool processConfigCommand(const nlohmann::json& cmd);
-    
-    // Utility functions
-    MotionCommand jsonToMotionCommand(const nlohmann::json& json);
-    nlohmann::json robotStateToJson(const RobotState& state);
-    nlohmann::json createStatusMessage();
-    
-    void startPeriodicTasks();
-    void stopPeriodicTasks();
-    
-    // Connection management
-    void handleMqttConnection(bool connected);
-    void reconnectMqtt();
-    
-    // Safety monitoring
-    void safetyMonitorLoop();
-    void checkEmergencyConditions();
-    
-    // Components
-    std::unique_ptr<Controller> robotController_;
-    std::unique_ptr<MqttClient> mqttClient_;
-    
-    // Configuration
-    std::string configFile_;
-    
-    // State
     std::atomic<bool> running_;
     std::atomic<bool> initialized_;
-    std::atomic<bool> mqttConnected_;
-    
-    // Threads
-    std::thread mainThread_;
-    std::thread safetyThread_;
-    std::vector<std::thread> periodicThreads_;
-    
-    // Timing
-    std::chrono::steady_clock::time_point lastHeartbeat_;
-    std::chrono::steady_clock::time_point lastStatePublish_;
-    std::chrono::steady_clock::time_point lastCommandReceived_;
-    
-    // Statistics
-    struct Statistics {
-        uint64_t commandsReceived = 0;
-        uint64_t commandsExecuted = 0;
-        uint64_t commandsFailed = 0;
-        uint64_t statesPublished = 0;
-        uint64_t errorsOccurred = 0;
-        std::chrono::steady_clock::time_point startTime;
-    } stats_;
-    
-    // Safety parameters
-    double maxIdleTime_ = 30.0; // seconds
-    double heartbeatInterval_ = 5.0; // seconds
-    double statePublishInterval_ = 0.02; // 50Hz
-    
-    // Current robot state cache
-    mutable std::mutex stateCacheMutex_;
-    RobotState lastKnownState_;
-    
-    // Request tracking
-    std::mutex requestMutex_;
-    std::map<std::string, std::chrono::steady_clock::time_point> pendingRequests_;
-    
-    // Error handling
-    std::mutex errorMutex_;
-    std::queue<std::string> errorQueue_;
-    void processErrorQueue();
 };
 
 } // namespace custom

src/config.cpp

@@ -3,7 +3,7 @@
 #include <fstream>
 #include <iostream>
 
-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_video = VIDEO_ENABLED;
-    config_.enable_audio = ENABLE_AUDIO;
+    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<int>(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;
+    using hp = ConfigParams<ConfigPreset::HighPerformance>;
-    config_.state_publish_frequency = HighPerformancePreset::STATE_PUBLISH_FREQUENCY;
+    config_.control_frequency = hp::control_frequency;
-    config_.max_linear_velocity = HighPerformancePreset::MAX_LINEAR_VELOCITY;
+    config_.state_publish_frequency = hp::state_publish_frequency;
-    config_.max_angular_velocity = HighPerformancePreset::MAX_ANGULAR_VELOCITY;
+    config_.max_linear_velocity = hp::max_linear_velocity;
-    config_.enable_video = HighPerformancePreset::ENABLE_VIDEO;
+    config_.max_angular_velocity = hp::max_angular_velocity;
-    config_.enable_audio = HighPerformancePreset::ENABLE_AUDIO;
+    config_.enable_video = hp::video_enabled;
+    config_.enable_audio = hp::audio_enabled;
 }
 
 void Config::loadDevelopmentPreset() {
     loadDefaults();
-    config_.control_frequency = DevelopmentPreset::CONTROL_FREQUENCY;
+    using dev = ConfigParams<ConfigPreset::Development>;
-    config_.state_publish_frequency = DevelopmentPreset::STATE_PUBLISH_FREQUENCY;
+    config_.control_frequency = dev::control_frequency;
-    config_.max_linear_velocity = DevelopmentPreset::MAX_LINEAR_VELOCITY;
+    config_.state_publish_frequency = dev::state_publish_frequency;
-    config_.max_angular_velocity = DevelopmentPreset::MAX_ANGULAR_VELOCITY;
+    config_.max_linear_velocity = dev::max_linear_velocity;
-    config_.enable_video = DevelopmentPreset::ENABLE_VIDEO;
+    config_.max_angular_velocity = dev::max_angular_velocity;
-    config_.enable_audio = DevelopmentPreset::ENABLE_AUDIO;
+    config_.enable_video = dev::video_enabled;
+    config_.enable_audio = dev::audio_enabled;
 }
 
 void Config::loadSafetyPreset() {
     loadDefaults();
-    config_.control_frequency = SafetyPreset::CONTROL_FREQUENCY;
+    using safe = ConfigParams<ConfigPreset::Safety>;
-    config_.state_publish_frequency = SafetyPreset::STATE_PUBLISH_FREQUENCY;
+    config_.control_frequency = safe::control_frequency;
-    config_.max_linear_velocity = SafetyPreset::MAX_LINEAR_VELOCITY;
+    config_.state_publish_frequency = safe::state_publish_frequency;
-    config_.max_angular_velocity = SafetyPreset::MAX_ANGULAR_VELOCITY;
+    config_.max_linear_velocity = safe::max_linear_velocity;
-    config_.emergency_stop_timeout = SafetyPreset::EMERGENCY_STOP_TIMEOUT;
+    config_.max_angular_velocity = safe::max_angular_velocity;
-    config_.enable_video = SafetyPreset::ENABLE_VIDEO;
+    config_.emergency_stop_timeout = safe::emergency_stop_timeout;
-    config_.enable_audio = SafetyPreset::ENABLE_AUDIO;
+    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
+    // For now just load defaults, JSON loading can be added later if needed
-    if (config::USE_COMPILE_TIME_CONFIG) {
+    loadDefaults();
-        loadDefaults();
+    
-        return true;
+    /* 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_width = VIDEO_WIDTH;
-        config_.video_height = config::VIDEO_HEIGHT;
+        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

src/controller.cpp

@@ -7,63 +7,34 @@
 namespace custom {
 
 Controller::Controller(const std::string& networkInterface)
-    : networkInterface_(networkInterface), running_(false), initialized_(false),
+    : networkInterface_(networkInterface), running_(false), initialized_(false) {
-      emergencyStopActive_(false) {
-    
-    auto& config = Config::getInstance().getConfig();
-    controlPeriod_ = std::chrono::milliseconds(static_cast<int>(1000.0 / config.control_frequency));
-    statePeriod_ = std::chrono::milliseconds(static_cast<int>(1000.0 / config.state_publish_frequency));
 }
 
 Controller::~Controller() {
-    shutdown();
+    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<unitree::robot::go2::SportClient>();
         sportClient_->SetTimeout(10.0f);
         sportClient_->Init();
         
-        // Initialize obstacle avoidance client
         obstacleClient_ = std::make_unique<unitree::robot::go2::ObstaclesAvoidClient>();
+        obstacleClient_->SetTimeout(3.0f);
         obstacleClient_->Init();
         
-        // Initialize video client
-        videoClient_ = std::make_unique<unitree::robot::go2::VideoClient>();
-        videoClient_->Init();
-        
-        // Initialize VUI client
-        vuiClient_ = std::make_unique<unitree::robot::go2::VuiClient>();
-        vuiClient_->Init();
-        
-        // Initialize state subscribers
-        sportStateSubscriber_.reset(
-            new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>("rt/sportmodestate")
-        );
-        sportStateSubscriber_->InitChannel(
-            std::bind(&Controller::updateSportModeState, this, std::placeholders::_1), 1
-        );
-        
-        lowStateSubscriber_.reset(
-            new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>("rt/lowstate")
-        );
-        lowStateSubscriber_->InitChannel(
-            std::bind(&Controller::updateLowLevelState, this, std::placeholders::_1), 1
-        );
-        
-        // Initialize robot state
-        {
-            std::lock_guard<std::mutex> lock(stateMutex_);
-            currentState_ = RobotState{};
-            currentState_.timestamp = std::chrono::steady_clock::now();
-        }
         
         initialized_ = true;
         LOG_INFO("Robot controller initialized successfully");
@@ -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<std::mutex> lock(stateMutex_);
-    return currentState_;
-}
 
-void Controller::setStateCallback(StateCallback callback) {
-    stateCallback_ = callback;
-}
 
-void Controller::setErrorCallback(ErrorCallback callback) {
-    errorCallback_ = callback;
-}
 
-bool Controller::executeCommand(const MotionCommand& command) {
+bool Controller::StandUp() {
-    if (!running_) {
-        LOG_ERROR("Cannot execute command: robot controller not running");
-        return false;
-    }
-    
-    if (emergencyStopActive_) {
-        LOG_WARN("Cannot execute command: emergency stop active");
-        return false;
-    }
-    
-    if (!validateCommand(command)) {
-        LOG_ERROR("Invalid motion command");
-        return false;
-    }
-    
-    {
-        std::lock_guard<std::mutex> lock(commandMutex_);
-        commandQueue_.push(command);
-        lastCommandTime_ = std::chrono::steady_clock::now();
-    }
-    commandCondition_.notify_one();
-    
-    return true;
-}
-
-bool Controller::emergencyStop() {
-    LOG_WARN("Emergency stop activated");
-    
-    emergencyStopActive_ = true;
-    
-    try {
-        if (sportClient_) {
-            sportClient_->StopMove();
-            sportClient_->Damp();
-        }
-        return true;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Emergency stop failed: " + std::string(e.what()));
-        return false;
-    }
-}
-
-bool Controller::recoveryStand() {
-    LOG_INFO("Executing recovery stand");
-    
-    try {
-        if (sportClient_) {
-            int32_t result = sportClient_->RecoveryStand();
-            if (result == 0) {
-                emergencyStopActive_ = false;
-                return true;
-            }
-        }
-        return false;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Recovery stand failed: " + std::string(e.what()));
-        return false;
-    }
-}
-
-// Basic motions
-bool Controller::standUp() {
     LOG_INFO("Standing up");
     try {
         return sportClient_ && sportClient_->StandUp() == 0;
@@ -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::RecoveryStand() {
-bool Controller::move(double vx, double vy, double vyaw) {
+    LOG_INFO("Executing recovery stand");
-    try {
-        auto& config = Config::getInstance().getConfig();
     
-        // Apply safety limits
-        vx = std::max(-config.max_linear_velocity, std::min(config.max_linear_velocity, vx));
-        vy = std::max(-config.max_linear_velocity, std::min(config.max_linear_velocity, vy));
-        vyaw = std::max(-config.max_angular_velocity, std::min(config.max_angular_velocity, vyaw));
-        
-        return sportClient_ && sportClient_->Move(vx, vy, vyaw) == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Move failed: " + std::string(e.what()));
-        return false;
-    }
-}
-
-bool Controller::moveToPosition(double x, double y, double yaw) {
-    try {
-        return obstacleClient_ && obstacleClient_->MoveToAbsolutePosition(x, y, yaw) == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Move to position failed: " + std::string(e.what()));
-        return false;
-    }
-}
-
-bool Controller::stop() {
-    try {
-        return sportClient_ && sportClient_->StopMove() == 0;
-    } catch (const std::exception& e) {
-        LOG_ERROR("Stop failed: " + std::string(e.what()));
-        return false;
-    }
-}
-
-// Body control
-bool Controller::setBodyPose(double roll, double pitch, double yaw, double height) {
     try {
         if (sportClient_) {
-            sportClient_->Euler(roll, pitch, yaw);
+            int32_t result = sportClient_->RecoveryStand();
-            sportClient_->BodyHeight(height);
+            return result == 0;
-            return true;
         }
         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();
+bool Controller::SwitchSet(bool enable) {
-        auto elapsed = endTime - startTime;
+    try {
-        
+        return obstacleClient_ && obstacleClient_->SwitchSet(enable) == 0;
-        if (elapsed < controlPeriod_) {
+    } catch (const std::exception& e) {
-            std::this_thread::sleep_for(controlPeriod_ - elapsed);
+        LOG_ERROR("Switch set failed: " + std::string(e.what()));
-        }
+        return false;
-    }
-    
-    LOG_INFO("Control loop stopped");
-}
-
-void Controller::stateUpdateLoop() {
-    LOG_INFO("State update loop started");
-    
-    while (running_) {
-        auto startTime = std::chrono::steady_clock::now();
-        
-        try {
-            // State is updated via callbacks, just trigger callback here
-            if (stateCallback_) {
-                RobotState state = getCurrentState();
-                stateCallback_(state);
-            }
-        } catch (const std::exception& e) {
-            LOG_ERROR("State update error: " + std::string(e.what()));
-        }
-        
-        auto endTime = std::chrono::steady_clock::now();
-        auto elapsed = endTime - startTime;
-        
-        if (elapsed < statePeriod_) {
-            std::this_thread::sleep_for(statePeriod_ - elapsed);
-        }
-    }
-    
-    LOG_INFO("State update loop stopped");
-}
-
-void Controller::processCommands() {
-    std::unique_lock<std::mutex> lock(commandMutex_);
-    
-    while (!commandQueue_.empty()) {
-        auto command = commandQueue_.front();
-        commandQueue_.pop();
-        lock.unlock();
-        
-        executeMotionCommand(command);
-        
-        lock.lock();
     }
 }
 
-void Controller::executeMotionCommand(const MotionCommand& command) {
+bool Controller::SwitchGet(bool& enable) {
-    switch (command.type) {
+    try {
-        case MotionCommand::VELOCITY:
+        return obstacleClient_ && obstacleClient_->SwitchGet(enable) == 0;
-            move(command.linear_velocity[0], command.linear_velocity[1], command.angular_velocity[2]);
+    } catch (const std::exception& e) {
-            break;
+        LOG_ERROR("Switch get failed: " + std::string(e.what()));
-            
+        return false;
-        case MotionCommand::POSITION:
-            moveToPosition(command.target_position[0], command.target_position[1], command.target_orientation[2]);
-            break;
-            
-        case MotionCommand::BODY_POSE:
-            setBodyPose(command.body_roll, command.body_pitch, command.body_yaw, command.body_height);
-            break;
-            
-        case MotionCommand::SPECIAL_ACTION:
-            performAction(command.action_name, command.action_params);
-            break;
-            
-        default:
-            LOG_WARN("Unknown motion command type");
-            break;
     }
 }
 
-bool Controller::validateCommand(const MotionCommand& command) {
+bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) {
-    auto& config = Config::getInstance().getConfig();
+    try {
-    
+        return obstacleClient_ && obstacleClient_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0;
-    // Check velocity limits
+    } catch (const std::exception& e) {
-    if (command.type == MotionCommand::VELOCITY) {
+        LOG_ERROR("Use remote command from api failed: " + std::string(e.what()));
-        double vx = std::abs(command.linear_velocity[0]);
+        return false;
-        double vy = std::abs(command.linear_velocity[1]);
-        double vyaw = std::abs(command.angular_velocity[2]);
-        
-        if (vx > config.max_linear_velocity || vy > config.max_linear_velocity || 
-            vyaw > config.max_angular_velocity) {
-            return false;
-        }
-    }
-    
-    return true;
-}
-
-void Controller::updateSportModeState(const void* message) {
-    auto* state = static_cast<const unitree_go::msg::dds_::SportModeState_*>(message);
-    
-    std::lock_guard<std::mutex> lock(stateMutex_);
-    
-    // Update position
-    currentState_.position[0] = state->position()[0];
-    currentState_.position[1] = state->position()[1];
-    currentState_.position[2] = state->position()[2];
-    
-    // Update orientation
-    currentState_.orientation[0] = state->imu_state().rpy()[0];
-    currentState_.orientation[1] = state->imu_state().rpy()[1];
-    currentState_.orientation[2] = state->imu_state().rpy()[2];
-    
-    // Update velocity
-    currentState_.velocity[0] = state->velocity()[0];
-    currentState_.velocity[1] = state->velocity()[1];
-    currentState_.velocity[2] = state->velocity()[2];
-    
-    // Update IMU
-    for (int i = 0; i < 3; i++) {
-        currentState_.imu_acc[i] = state->imu_state().accelerometer()[i];
-        currentState_.imu_gyro[i] = state->imu_state().gyroscope()[i];
-    }
-    
-    for (int i = 0; i < 4; i++) {
-        currentState_.imu_quat[i] = state->imu_state().quaternion()[i];
-    }
-    
-    currentState_.timestamp = std::chrono::steady_clock::now();
-    currentState_.is_connected = true;
-}
-
-void Controller::updateLowLevelState(const void* message) {
-    auto* state = static_cast<const unitree_go::msg::dds_::LowState_*>(message);
-    
-    std::lock_guard<std::mutex> lock(stateMutex_);
-    
-    // Update motor states
-    for (int i = 0; i < 20 && i < state->motor_state().size(); i++) {
-        currentState_.motor_positions[i] = state->motor_state()[i].q();
-        currentState_.motor_velocities[i] = state->motor_state()[i].dq();
-        currentState_.motor_torques[i] = state->motor_state()[i].tau_est();
-        currentState_.motor_temperatures[i] = state->motor_state()[i].temperature();
-    }
-    
-    // Update battery info
-    if (!state->bms_state().empty()) {
-        currentState_.battery_voltage = state->bms_state()[0].voltage();
-        currentState_.battery_current = state->bms_state()[0].current();
-        currentState_.battery_percentage = state->bms_state()[0].soc();
-    }
-    
-    currentState_.timestamp = std::chrono::steady_clock::now();
-}
-
-void Controller::checkSafetyLimits() {
-    auto& config = Config::getInstance().getConfig();
-    auto now = std::chrono::steady_clock::now();
-    
-    // Check command timeout
-    auto timeSinceLastCommand = std::chrono::duration_cast<std::chrono::seconds>(
-        now - lastCommandTime_).count();
-    
-    if (timeSinceLastCommand > config.emergency_stop_timeout && !emergencyStopActive_) {
-        LOG_WARN("Command timeout exceeded, activating emergency stop");
-        emergencyStop();
-    }
-    
-    // Check battery level
-    {
-        std::lock_guard<std::mutex> lock(stateMutex_);
-        if (currentState_.battery_percentage < 10) {
-            currentState_.low_battery = true;
-            if (!emergencyStopActive_) {
-                LOG_WARN("Low battery detected, activating emergency stop");
-                emergencyStop();
-            }
-        }
     }
 }
 
-void Controller::handleError(const std::string& error) {
+bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) {
-    if (errorCallback_) {
+    try {
-        errorCallback_(error);
+        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) {
+bool Controller::MoveToIncrementPosition(float x, float y, float yaw) {
-    LOG_INFO("Performing action: " + actionName);
+    try {
-    
+        return obstacleClient_ && obstacleClient_->MoveToIncrementPosition(x, y, yaw) == 0;
-    if (actionName == "dance1") return dance1();
+    } catch (const std::exception& e) {
-    if (actionName == "dance2") return dance2();
+        LOG_ERROR("Move to increment position failed: " + std::string(e.what()));
-    if (actionName == "hello") return hello();
+        return false;
-    if (actionName == "sit") return sit();
+    }
-    if (actionName == "stand_up") return standUp();
-    if (actionName == "stand_down") return standDown();
-    if (actionName == "damp") return damp();
-    if (actionName == "balance_stand") return balanceStand();
-    if (actionName == "recovery_stand") return recoveryStand();
-    
-    LOG_WARN("Unknown action: " + actionName);
-    return false;
 }
 
 } // namespace custom

src/custom_robot.cpp

@@ -1,71 +1,48 @@
 #include "custom_robot.hpp"
+#include <thread>
 #include <chrono>
-#include <sstream>
+#include <functional>
 
 namespace custom {
 
-CustomRobot::CustomRobot(const std::string& configFile)
+CustomRobot::CustomRobot()
-    : configFile_(configFile.empty() ? "config/robot_config.json" : configFile),
+    : initialized_(false), running_(false) {
-      running_(false), initialized_(false), mqttConnected_(false) {
 
-    stats_.startTime = std::chrono::steady_clock::now();
+    }
-}
 
 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
+    controller_ = std::make_unique<Controller>(config.network_interface);
-    robotController_ = std::make_unique<Controller>(config.network_interface);
+    if (!controller_->initialize()) {
-    if (!robotController_->initialize()) {
         LOG_ERROR("Failed to initialize robot controller");
         return false;
     }
     
-    // Set robot controller callbacks
+    // Initialize robot state client
-    robotController_->setStateCallback(
+    try {
-        std::bind(&CustomRobot::handleRobotState, this, std::placeholders::_1)
+        stateClient_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
-    );
+        stateClient_->SetTimeout(10.0f);
-    robotController_->setErrorCallback(
+        stateClient_->Init();
-        std::bind(&CustomRobot::handleRobotError, this, std::placeholders::_1)
+        LOG_INFO("RobotStateClient initialized successfully");
-    );
+    } catch (const std::exception& e) {
-    
+        LOG_ERROR("Failed to initialize RobotStateClient: " + std::string(e.what()));
-    // Initialize MQTT client
-    mqttClient_ = std::make_unique<MqttClient>(
-        config.mqtt_broker, config.mqtt_port, config.mqtt_client_id
-    );
-    
-    // Set MQTT callbacks
-    mqttClient_->setMessageCallback(
-        std::bind(&CustomRobot::handleMqttMessage, this, std::placeholders::_1, std::placeholders::_2)
-    );
-    mqttClient_->setConnectionCallback(
-        std::bind(&CustomRobot::handleMqttConnection, this, std::placeholders::_1)
-    );
-    
-    // Connect to MQTT broker
-    if (!mqttClient_->connect(config.mqtt_username, config.mqtt_password)) {
-        LOG_ERROR("Failed to connect to MQTT broker");
-        return false;
-    }
-    
-    // Start MQTT message processor
-    mqttClient_->startMessageProcessor();
-    
-    // Subscribe to command topics
-    std::string cmdTopic = Config::getInstance().getFullTopic(config.cmd_topic);
-    if (!mqttClient_->subscribe(cmdTopic + "/+")) { // Subscribe to all command subtopics
-        LOG_ERROR("Failed to subscribe to command topics");
         return false;
     }
     
@@ -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");
+    if (!controller_->start()) {
-    
-    // Start robot controller
-    if (!robotController_->start()) {
         LOG_ERROR("Failed to start robot controller");
         return false;
     }
-    
     running_ = true;
-    
-    // Start periodic tasks
-    startPeriodicTasks();
-    
-    // Publish initial status
-    publishHeartbeat();
-    
     LOG_INFO("CustomRobot started successfully");
     return true;
 }
 
-bool CustomRobot::stop() {
+bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
-    if (!running_) {
+    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::milliseconds>(
-            std::chrono::system_clock::now().time_since_epoch()).count();
-        
-        auto& config = Config::getInstance().getConfig();
-        std::string statusTopic = Config::getInstance().getFullTopic(config.state_topic + "/status");
-        mqttClient_->publishJson(statusTopic, status);
-    }
-    
-    LOG_INFO("CustomRobot stopped");
-    return true;
-}
-
-void CustomRobot::run() {
-    if (!start()) {
-        LOG_ERROR("Failed to start CustomRobot");
-        return;
-    }
-    
-    LOG_INFO("CustomRobot main loop started");
-    
-    while (running_) {
-        try {
-            // Process any pending errors
-            processErrorQueue();
-            
-            // Sleep for a short period
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
-            
-        } catch (const std::exception& e) {
-            LOG_ERROR("Main loop error: " + std::string(e.what()));
-        }
-    }
-    
-    LOG_INFO("CustomRobot main loop ended");
-}
-
-void CustomRobot::runAsync() {
-    mainThread_ = std::thread(&CustomRobot::run, this);
-}
-
-void CustomRobot::waitForShutdown() {
-    if (mainThread_.joinable()) {
-        mainThread_.join();
-    }
-}
-
-void CustomRobot::handleMqttMessage(const std::string& topic, const std::string& payload) {
-    try {
-        LOG_DEBUG("Received MQTT message on topic: " + topic);
-        
-        auto& config = Config::getInstance().getConfig();
-        std::string cmdPrefix = Config::getInstance().getFullTopic(config.cmd_topic);
-        
-        if (topic.find(cmdPrefix) == 0) {
-            // Parse JSON payload
-            nlohmann::json message = nlohmann::json::parse(payload);
-            
-            // Extract command type from topic
-            std::string cmdType = topic.substr(cmdPrefix.length() + 1);
-            
-            if (cmdType == "motion") {
-                handleCommandMessage(message);
-            } else if (cmdType == "config") {
-                handleConfigMessage(message);
-            } else if (cmdType == "control") {
-                handleControlMessage(message);
-            } else {
-                LOG_WARN("Unknown command type: " + cmdType);
-            }
-            
-            stats_.commandsReceived++;
-        }
-        
-    } catch (const nlohmann::json::parse_error& e) {
-        LOG_ERROR("JSON parse error: " + std::string(e.what()));
-        publishError("Invalid JSON in message: " + std::string(e.what()));
     } catch (const std::exception& e) {
-        LOG_ERROR("Message handling error: " + std::string(e.what()));
+        LOG_ERROR("Exception in getServiceList: " + std::string(e.what()));
-        publishError("Message handling error: " + std::string(e.what()));
+        return false;
     }
 }
 
-void CustomRobot::handleCommandMessage(const nlohmann::json& command) {
+bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
-    std::string requestId = command.value("request_id", "");
+    if (!initialized_ || !stateClient_) {
-    
+        LOG_ERROR("Robot not initialized or stateClient is null");
-    try {
-        if (!command.contains("type")) {
-            publishResponse(requestId, false, "Missing command type");
-            return;
-        }
-        
-        std::string cmdType = command["type"];
-        bool success = false;
-        
-        if (cmdType == "motion") {
-            success = processMotionCommand(command);
-        } else if (cmdType == "special_action") {
-            success = processSpecialAction(command);
-        } else if (cmdType == "system") {
-            success = processSystemCommand(command);
-        } else {
-            publishResponse(requestId, false, "Unknown command type: " + cmdType);
-            return;
-        }
-        
-        if (success) {
-            stats_.commandsExecuted++;
-            publishResponse(requestId, true);
-        } else {
-            stats_.commandsFailed++;
-            publishResponse(requestId, false, "Command execution failed");
-        }
-        
-        lastCommandReceived_ = std::chrono::steady_clock::now();
-        
-    } catch (const std::exception& e) {
-        LOG_ERROR("Command processing error: " + std::string(e.what()));
-        publishResponse(requestId, false, "Command processing error: " + std::string(e.what()));
-        stats_.commandsFailed++;
-    }
-}
-
-bool CustomRobot::processMotionCommand(const nlohmann::json& cmd) {
-    if (!robotController_) {
         return false;
     }
     
-    MotionCommand motionCmd = jsonToMotionCommand(cmd);
+    try {
-    return robotController_->executeCommand(motionCmd);
+        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) {
+bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
-    if (!robotController_ || !cmd.contains("action")) {
+    if (!initialized_ || !stateClient_) {
+        LOG_ERROR("Robot not initialized or stateClient is null");
         return false;
     }
     
-    std::string action = cmd["action"];
+    try {
-    nlohmann::json params = cmd.value("params", nlohmann::json::object());
+        int32_t ret = stateClient_->SetReportFreq(interval, duration);
-    
+        if (ret != 0) {
-    return robotController_->performAction(action, params);
+            LOG_ERROR("Failed to set report frequency, error code: " + std::to_string(ret));
-}
+            return false;
-
+        }
-bool CustomRobot::processSystemCommand(const nlohmann::json& cmd) {
+        LOG_INFO("Successfully set report frequency: interval=" + std::to_string(interval) + ", duration=" + std::to_string(duration));
-    if (!cmd.contains("command")) {
+        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<std::mutex> lock(stateCacheMutex_);
-        lastKnownState_ = state;
-    }
-    
-    // Publish state if enough time has passed
-    auto now = std::chrono::steady_clock::now();
-    auto timeSinceLastPublish = std::chrono::duration<double>(now - lastStatePublish_).count();
-    
-    if (timeSinceLastPublish >= statePublishInterval_) {
-        publishState();
-        lastStatePublish_ = now;
-    }
-}
-
-void CustomRobot::handleRobotError(const std::string& error) {
-    {
-        std::lock_guard<std::mutex> lock(errorMutex_);
-        errorQueue_.push(error);
-    }
-    
-    stats_.errorsOccurred++;
-}
-
-void CustomRobot::publishState() {
-    if (!mqttClient_ || !mqttClient_->isConnected()) {
-        return;
-    }
-    
-    try {
-        RobotState state;
-        {
-            std::lock_guard<std::mutex> lock(stateCacheMutex_);
-            state = lastKnownState_;
-        }
-        
-        nlohmann::json stateJson = robotStateToJson(state);
-        
-        auto& config = Config::getInstance().getConfig();
-        std::string stateTopic = Config::getInstance().getFullTopic(config.state_topic + "/robot");
-        
-        mqttClient_->publishJson(stateTopic, stateJson);
-        stats_.statesPublished++;
-        
-    } catch (const std::exception& e) {
-        LOG_ERROR("State publishing error: " + std::string(e.what()));
-    }
-}
-
-void CustomRobot::publishHeartbeat() {
-    if (!mqttClient_ || !mqttClient_->isConnected()) {
-        return;
-    }
-    
-    try {
-        nlohmann::json heartbeat = createStatusMessage();
-        
-        auto& config = Config::getInstance().getConfig();
-        std::string heartbeatTopic = Config::getInstance().getFullTopic(config.state_topic + "/heartbeat");
-        
-        mqttClient_->publishJson(heartbeatTopic, heartbeat);
-        lastHeartbeat_ = std::chrono::steady_clock::now();
-        
-    } catch (const std::exception& e) {
-        LOG_ERROR("Heartbeat publishing error: " + std::string(e.what()));
-    }
-}
-
-void CustomRobot::publishError(const std::string& error) {
-    if (!mqttClient_ || !mqttClient_->isConnected()) {
-        return;
-    }
-    
-    try {
-        nlohmann::json errorMsg;
-        errorMsg["error"] = error;
-        errorMsg["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
-            std::chrono::system_clock::now().time_since_epoch()).count();
-        
-        auto& config = Config::getInstance().getConfig();
-        std::string errorTopic = Config::getInstance().getFullTopic(config.state_topic + "/error");
-        
-        mqttClient_->publishJson(errorTopic, errorMsg);
-        
-    } catch (const std::exception& e) {
-        LOG_ERROR("Error publishing error: " + std::string(e.what()));
-    }
-}
-
-void CustomRobot::publishResponse(const std::string& requestId, bool success, const std::string& message) {
-    if (!mqttClient_ || !mqttClient_->isConnected() || requestId.empty()) {
-        return;
-    }
-    
-    try {
-        nlohmann::json response;
-        response["request_id"] = requestId;
-        response["success"] = success;
-        response["message"] = message;
-        response["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
-            std::chrono::system_clock::now().time_since_epoch()).count();
-        
-        auto& config = Config::getInstance().getConfig();
-        std::string responseTopic = Config::getInstance().getFullTopic(config.state_topic + "/response");
-        
-        mqttClient_->publishJson(responseTopic, response);
-        
-    } catch (const std::exception& e) {
-        LOG_ERROR("Response publishing error: " + std::string(e.what()));
-    }
-}
-
-MotionCommand CustomRobot::jsonToMotionCommand(const nlohmann::json& json) {
-    MotionCommand cmd;
-    cmd.timestamp = std::chrono::steady_clock::now();
-    
-    if (json.contains("motion_type")) {
-        std::string motionType = json["motion_type"];
-        if (motionType == "velocity") {
-            cmd.type = MotionCommand::VELOCITY;
-        } else if (motionType == "position") {
-            cmd.type = MotionCommand::POSITION;
-        } else if (motionType == "body_pose") {
-            cmd.type = MotionCommand::BODY_POSE;
-        } else if (motionType == "special_action") {
-            cmd.type = MotionCommand::SPECIAL_ACTION;
-        }
-    }
-    
-    // Parse velocity command
-    if (json.contains("linear_velocity")) {
-        auto vel = json["linear_velocity"];
-        if (vel.is_array() && vel.size() >= 3) {
-            cmd.linear_velocity[0] = vel[0];
-            cmd.linear_velocity[1] = vel[1];
-            cmd.linear_velocity[2] = vel[2];
-        }
-    }
-    
-    if (json.contains("angular_velocity")) {
-        auto vel = json["angular_velocity"];
-        if (vel.is_array() && vel.size() >= 3) {
-            cmd.angular_velocity[0] = vel[0];
-            cmd.angular_velocity[1] = vel[1];
-            cmd.angular_velocity[2] = vel[2];
-        }
-    }
-    
-    // Parse position command
-    if (json.contains("target_position")) {
-        auto pos = json["target_position"];
-        if (pos.is_array() && pos.size() >= 3) {
-            cmd.target_position[0] = pos[0];
-            cmd.target_position[1] = pos[1];
-            cmd.target_position[2] = pos[2];
-        }
-    }
-    
-    // Parse body pose
-    if (json.contains("body_height")) cmd.body_height = json["body_height"];
-    if (json.contains("body_roll")) cmd.body_roll = json["body_roll"];
-    if (json.contains("body_pitch")) cmd.body_pitch = json["body_pitch"];
-    if (json.contains("body_yaw")) cmd.body_yaw = json["body_yaw"];
-    
-    // Parse special action
-    if (json.contains("action_name")) {
-        cmd.action_name = json["action_name"];
-    }
-    if (json.contains("action_params")) {
-        cmd.action_params = json["action_params"];
-    }
-    
-    // Parse duration
-    if (json.contains("duration")) {
-        cmd.duration = json["duration"];
-    }
-    
-    return cmd;
-}
-
-nlohmann::json CustomRobot::robotStateToJson(const RobotState& state) {
-    nlohmann::json json;
-    
-    // Basic info
-    json["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
-        state.timestamp.time_since_epoch()).count();
-    json["is_connected"] = state.is_connected;
-    json["emergency_stop"] = state.emergency_stop;
-    
-    // Position and orientation
-    json["position"] = {state.position[0], state.position[1], state.position[2]};
-    json["orientation"] = {state.orientation[0], state.orientation[1], state.orientation[2]};
-    json["velocity"] = {state.velocity[0], state.velocity[1], state.velocity[2]};
-    
-    // IMU data
-    json["imu"]["acceleration"] = {state.imu_acc[0], state.imu_acc[1], state.imu_acc[2]};
-    json["imu"]["gyroscope"] = {state.imu_gyro[0], state.imu_gyro[1], state.imu_gyro[2]};
-    json["imu"]["quaternion"] = {state.imu_quat[0], state.imu_quat[1], state.imu_quat[2], state.imu_quat[3]};
-    
-    // Battery info
-    json["battery"]["voltage"] = state.battery_voltage;
-    json["battery"]["current"] = state.battery_current;
-    json["battery"]["percentage"] = state.battery_percentage;
-    json["battery"]["low_battery"] = state.low_battery;
-    
-    // System info
-    json["temperature"] = state.temperature;
-    json["overheated"] = state.overheated;
-    
-    // Motor states (first 12 leg motors)
-    json["motors"]["positions"] = std::vector<double>(state.motor_positions, state.motor_positions + 12);
-    json["motors"]["velocities"] = std::vector<double>(state.motor_velocities, state.motor_velocities + 12);
-    json["motors"]["torques"] = std::vector<double>(state.motor_torques, state.motor_torques + 12);
-    json["motors"]["temperatures"] = std::vector<double>(state.motor_temperatures, state.motor_temperatures + 12);
-    
-    return json;
-}
-
-nlohmann::json CustomRobot::createStatusMessage() {
-    nlohmann::json status;
-    
-    status["status"] = running_ ? "running" : "stopped";
-    status["initialized"] = initialized_;
-    status["mqtt_connected"] = mqttConnected_;
-    status["robot_connected"] = robotController_ && robotController_->isRunning();
-    
-    // Statistics
-    status["stats"]["commands_received"] = stats_.commandsReceived;
-    status["stats"]["commands_executed"] = stats_.commandsExecuted;
-    status["stats"]["commands_failed"] = stats_.commandsFailed;
-    status["stats"]["states_published"] = stats_.statesPublished;
-    status["stats"]["errors_occurred"] = stats_.errorsOccurred;
-    
-    // Uptime
-    auto now = std::chrono::steady_clock::now();
-    auto uptime = std::chrono::duration_cast<std::chrono::seconds>(now - stats_.startTime).count();
-    status["uptime_seconds"] = uptime;
-    
-    status["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
-        std::chrono::system_clock::now().time_since_epoch()).count();
-    
-    return status;
-}
-
-void CustomRobot::startPeriodicTasks() {
-    // Heartbeat thread
-    periodicThreads_.emplace_back([this]() {
-        while (running_) {
-            auto now = std::chrono::steady_clock::now();
-            auto timeSinceLastHeartbeat = std::chrono::duration<double>(now - lastHeartbeat_).count();
-            
-            if (timeSinceLastHeartbeat >= heartbeatInterval_) {
-                publishHeartbeat();
-            }
-            
-            std::this_thread::sleep_for(std::chrono::seconds(1));
-        }
-    });
-    
-    // Safety monitor thread
-    safetyThread_ = std::thread(&CustomRobot::safetyMonitorLoop, this);
-}
-
-void CustomRobot::stopPeriodicTasks() {
-    // Wait for all periodic threads to finish
-    for (auto& thread : periodicThreads_) {
-        if (thread.joinable()) {
-            thread.join();
-        }
-    }
-    periodicThreads_.clear();
-    
-    if (safetyThread_.joinable()) {
-        safetyThread_.join();
-    }
-}
-
-void CustomRobot::handleMqttConnection(bool connected) {
-    mqttConnected_ = connected;
-    
-    if (connected) {
-        LOG_INFO("MQTT connection established");
-        
-        // Resubscribe to topics
-        auto& config = Config::getInstance().getConfig();
-        std::string cmdTopic = Config::getInstance().getFullTopic(config.cmd_topic);
-        mqttClient_->subscribe(cmdTopic + "/+");
-        
-        // Publish connection status
-        publishHeartbeat();
-    } else {
-        LOG_WARN("MQTT connection lost");
-    }
-}
-
-void CustomRobot::safetyMonitorLoop() {
-    while (running_) {
-        try {
-            checkEmergencyConditions();
-            std::this_thread::sleep_for(std::chrono::milliseconds(500));
-        } catch (const std::exception& e) {
-            LOG_ERROR("Safety monitor error: " + std::string(e.what()));
-        }
-    }
-}
-
-void CustomRobot::checkEmergencyConditions() {
-    // Check command timeout
-    auto now = std::chrono::steady_clock::now();
-    auto timeSinceLastCommand = std::chrono::duration<double>(now - lastCommandReceived_).count();
-    
-    if (timeSinceLastCommand > maxIdleTime_ && robotController_ && robotController_->isRunning()) {
-        LOG_WARN("Maximum idle time exceeded, stopping robot");
-        robotController_->emergencyStop();
-    }
-}
-
-void CustomRobot::processErrorQueue() {
-    std::lock_guard<std::mutex> lock(errorMutex_);
-    
-    while (!errorQueue_.empty()) {
-        std::string error = errorQueue_.front();
-        errorQueue_.pop();
-        
-        publishError(error);
-    }
-}
-
-} // namespace custom

src/main.cpp

@@ -11,24 +11,18 @@ std::unique_ptr<CustomRobot> g_robot;
 
 void signalHandler(int signal) {
     LOG_INFO("Received signal " + std::to_string(signal) + ", shutting down...");
-    if (g_robot) {
-        g_robot->stop();
-    }
     exit(0);
 }
 
 int main(int argc, char** argv) {
-    // Initialize logger with default settings
     Logger::getInstance().setLevel(LogLevel::INFO);
     
     LOG_INFO("Starting Unitree GO2 System v0.0.1");
     
-    // Setup signal handlers
     signal(SIGINT, signalHandler);
     signal(SIGTERM, signalHandler);
     
     try {
-        // Create robot with compile-time defaults (no config file needed)
         g_robot = std::make_unique<CustomRobot>("");
         
         if (!g_robot->initialize()) {
@@ -37,11 +31,7 @@ int main(int argc, char** argv) {
         }
         
         LOG_INFO("Robot initialized successfully");
-        LOG_INFO("Press Ctrl+C to stop the robot");
+        g_robot->start();
-        
-        // Run the robot
-        g_robot->run();
-        
     } catch (const std::exception& e) {
         LOG_ERROR("Exception: " + std::string(e.what()));
         return 1;