Implement MQTT functionality in CustomRobot class. Added methods for MQTT initialization, message handling, and connection management. Updated configuration to remove multimedia settings and streamline MQTT-related parameters. Enhanced logging for MQTT operations and ensured proper cleanup during shutdown.

include/config.hpp

@@ -29,8 +29,6 @@ constexpr std::string_view MQTT_PASSWORD = "";
 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
 constexpr double CONTROL_FREQUENCY = 200.0;       // Hz
@@ -45,18 +43,6 @@ constexpr double EMERGENCY_STOP_TIMEOUT = 5.0;    // seconds
 constexpr double STAND_HEIGHT = 0.0;              // relative height
 constexpr Gait DEFAULT_GAIT = Gait::IDLE;
 
-// Video settings
-constexpr int VIDEO_WIDTH = 1280;
-constexpr int VIDEO_HEIGHT = 720;
-constexpr int VIDEO_FPS = 30;
-constexpr std::string_view VIDEO_FORMAT = "h264";
-constexpr bool VIDEO_ENABLED = true;
-
-// Audio settings
-constexpr int AUDIO_SAMPLE_RATE = 16000;
-constexpr int AUDIO_CHANNELS = 1;
-constexpr std::string_view AUDIO_FORMAT = "pcm";
-constexpr bool AUDIO_ENABLED = true;
 
 class CustomConfig {
 public:
@@ -72,8 +58,6 @@ public:
     std::string topic_prefix;
     std::string topic_cmd;
     std::string topic_state;
-    std::string topic_video;
-    std::string topic_audio;
     
     // 机器人控制和安全
     double control_frequency;
@@ -84,16 +68,6 @@ public:
     double stand_height;
     int default_gait;
     
-    // 多媒体
-    bool enable_video;
-    bool enable_audio;
-    int video_width;
-    int video_height;
-    int video_fps;
-    std::string video_format;
-    int audio_sample_rate;
-    int audio_channels;
-    std::string audio_format;
     
     void loadDefaults();
 

include/custom_robot.hpp

@@ -3,6 +3,7 @@
 #include "controller.hpp"
 #include "config.hpp"
 #include "logger.hpp"
+#include "mqtt.hpp"
 
 #include <memory>
 #include <atomic>
@@ -22,14 +23,21 @@ public:
     
     Controller* getController() const { return controller_.get(); }
     
-    // Robot state methods
     bool GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList);
     bool SwitchService(const std::string& serviceName, bool enable);
     bool SetReportFreq(int32_t interval, int32_t duration);
     
+    // MQTT related methods
+    bool initializeMqtt();
+    void onMqttMessage(const std::string& topic, const std::string& payload);
+    void onMqttConnection(bool connected);
+    void publishStatus();
+
 private:
+    std::string generateRandomClientId() const;
     std::unique_ptr<Controller> controller_;
     std::unique_ptr<unitree::robot::go2::RobotStateClient> rsc_;
+    std::unique_ptr<MqttClient> mqttClient_;
     
     CustomConfig config_;  
     std::atomic<bool> running_;

src/config.cpp

@@ -3,7 +3,6 @@
 namespace custom {
 
 void CustomConfig::loadDefaults() {
-    // 网络和MQTT连接
     network_interface = std::string(NETWORK_INTERFACE);
     mqtt_broker = std::string(MQTT_BROKER);
     mqtt_port = MQTT_PORT;
@@ -11,14 +10,10 @@ void CustomConfig::loadDefaults() {
     mqtt_password = std::string(MQTT_PASSWORD);
     mqtt_client_id = std::string(MQTT_CLIENT_ID);
     
-    // MQTT话题
     topic_prefix = std::string(TOPIC_PREFIX);
     topic_cmd = std::string(TOPIC_CMD);
     topic_state = std::string(TOPIC_STATE);
-    topic_video = std::string(TOPIC_VIDEO);
-    topic_audio = std::string(TOPIC_AUDIO);
     
-    // 机器人控制和安全
     control_frequency = CONTROL_FREQUENCY;
     state_publish_frequency = STATE_PUBLISH_FREQUENCY;
     max_linear_velocity = MAX_LINEAR_VELOCITY;
@@ -27,16 +22,6 @@ void CustomConfig::loadDefaults() {
     stand_height = STAND_HEIGHT;
     default_gait = static_cast<int>(DEFAULT_GAIT);
     
-    // 多媒体
-    enable_video = VIDEO_ENABLED;
-    enable_audio = AUDIO_ENABLED;
-    video_width = VIDEO_WIDTH;
-    video_height = VIDEO_HEIGHT;
-    video_fps = VIDEO_FPS;
-    video_format = std::string(VIDEO_FORMAT);
-    audio_sample_rate = AUDIO_SAMPLE_RATE;
-    audio_channels = AUDIO_CHANNELS;
-    audio_format = std::string(AUDIO_FORMAT);
 }
 
 } // namespace custom

src/custom_robot.cpp

@@ -2,13 +2,17 @@
 #include <thread>
 #include <chrono>
 #include <functional>
+#include <ctime>
+#include <random>
+#include <sstream>
+#include <iomanip>
+#include <nlohmann/json.hpp>
 #include <unitree/robot/channel/channel_factory.hpp>
 
 namespace custom {
 
 CustomRobot::CustomRobot()
     : initialized_(false), running_(false) {
-    // 创建并加载配置
     config_.loadDefaults();
     
     try {
@@ -21,6 +25,15 @@ CustomRobot::CustomRobot()
 
 CustomRobot::~CustomRobot() {
     LOG_INFO("Shutting down CustomRobot");
+    
+    // Cleanup MQTT client
+    if (mqttClient_) {
+        mqttClient_->stopMessageProcessor();
+        mqttClient_->disconnect();
+        mqttClient_.reset();
+        LOG_INFO("MQTT client disconnected and cleaned up");
+    }
+    
     if (controller_) {
         controller_->shutdown();
         controller_.reset();
@@ -48,6 +61,12 @@ bool CustomRobot::initialize() {
         rsc_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
         rsc_->SetTimeout(3.0);
         rsc_->Init();
+        
+        // Initialize MQTT client
+        if (!initializeMqtt()) {
+            LOG_ERROR("Failed to initialize MQTT client");
+            return false;
+        }
     } catch (const std::exception& e) {
         LOG_ERROR("Failed to initialize : " + std::string(e.what()));
         return false;
@@ -157,4 +176,149 @@ bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
     }
 }
 
+bool CustomRobot::initializeMqtt() {
+    try {
+        std::string broker = config_->mqtt_broker;
+        int port = config_->mqtt_port;
+        std::string clientId = generateRandomClientId();  
+        
+        mqttClient_ = std::make_unique<MqttClient>(broker, port, clientId);
+        using namespace std::placeholders;
+        mqttClient_->setMessageCallback(std::bind(&CustomRobot::onMqttMessage, this, _1, _2));
+        mqttClient_->setConnectionCallback(std::bind(&CustomRobot::onMqttConnection, this, _1));
+        
+        mqttClient_->startMessageProcessor();
+        
+        if (!mqttClient_->connect()) {
+            LOG_ERROR("Failed to connect to MQTT broker");
+            return false;
+        }
+        
+        std::string controlTopic = config_->topic_prefix + "/" + config_->topic_cmd;
+        
+        mqttClient_->subscribe(controlTopic, 1);
+        
+        LOG_INFO("MQTT client initialized successfully");
+        return true;
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Exception during MQTT initialization: " + std::string(e.what()));
+        return false;
+    }
+}
+
+void CustomRobot::onMqttMessage(const std::string& topic, const std::string& payload) {
+    try {
+        LOG_INFO("Received MQTT message on topic: " + topic + ", payload: " + payload);
+
+        // nlohmann::json message = nlohmann::json::parse(payload);
+        
+        // std::string controlTopic = config_->topic_prefix + "/" + config_->topic_cmd;
+        
+        // if (topic == controlTopic) {
+        //     if (message.contains("command")) {
+        //         std::string command = message["command"];
+                
+        //         if (command == "stand" && controller_) {
+        //             controller_->standUp();
+                    
+        //             // Publish status response
+        //             nlohmann::json response;
+        //             response["status"] = "executed";
+        //             response["command"] = command;
+        //             response["timestamp"] = std::time(nullptr);
+        //             std::string statusTopic = config_->topic_prefix + "/" + config_->topic_state;
+        //             mqttClient_->publishJson(statusTopic, response);
+                    
+        //             // Publish updated status
+        //             publishStatus();
+                    
+        //         } else if (command == "sit" && controller_) {
+        //             controller_->sitDown();
+                    
+        //             nlohmann::json response;
+        //             response["status"] = "executed";
+        //             response["command"] = command;
+        //             response["timestamp"] = std::time(nullptr);
+        //             std::string statusTopic = config_->topic_prefix + "/" + config_->topic_state;
+        //             mqttClient_->publishJson(statusTopic, response);
+                    
+        //             // Publish updated status
+        //             publishStatus();
+        //         }
+        //     } else {
+        //         // Unknown command, send error response
+        //         nlohmann::json response;
+        //         response["status"] = "error";
+        //         response["message"] = "Unknown command: " + command;
+        //         response["timestamp"] = std::time(nullptr);
+        //         std::string statusTopic = config_->topic_prefix + "/" + config_->topic_state;
+        //         mqttClient_->publishJson(statusTopic, response);
+                
+        //         // Publish updated status
+        //         publishStatus();
+        //     }
+        // }
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error processing MQTT message: " + std::string(e.what()));
+    }
+}
+
+void CustomRobot::onMqttConnection(bool connected) {
+    if (connected) {
+        LOG_INFO("MQTT client connected to broker");
+        
+        nlohmann::json status;
+        status["status"] = "online";
+        status["timestamp"] = std::time(nullptr);
+        std::string statusTopic = config_->topic_prefix + "/" + config_->topic_state;
+        mqttClient_->publishJson(statusTopic, status, 1, true);
+        
+        publishStatus();
+        
+    } else {
+        LOG_WARN("MQTT client disconnected from broker");
+    }
+}
+
+void CustomRobot::publishStatus() {
+    if (!mqttClient_ || !mqttClient_->isConnected()) {
+        return;
+    }
+    
+    try {
+        nlohmann::json status;
+        status["initialized"] = initialized_.load();
+        status["running"] = running_.load();
+        status["mqtt_connected"] = mqttClient_->isConnected();
+        status["timestamp"] = std::time(nullptr);
+        
+        if (controller_) {
+            status["controller_connected"] = controller_->isConnected();
+        }
+        
+        std::string statusTopic = config_->topic_prefix + "/" + config_->topic_state;
+        mqttClient_->publishJson(statusTopic, status, 1, true);
+        
+        LOG_DEBUG("Published status to topic: " + statusTopic);
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Failed to publish status: " + std::string(e.what()));
+    }
+}
+
+std::string CustomRobot::generateRandomClientId() const {
+    std::string baseId = config_->mqtt_client_id;
+    
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    std::uniform_int_distribution<> dis(1000, 9999);
+    
+    std::stringstream ss;
+    ss << baseId << "_" << std::time(nullptr) << "_" << dis(gen);
+    
+    return ss.str();
+}
+
 } // namespace custom