From 90228e4ab949508d11f5b121c05a417577562e68 Mon Sep 17 00:00:00 2001 From: Sucan126 <632190820@qq.com> Date: Mon, 8 Sep 2025 18:35:02 +0800 Subject: [PATCH] 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 | 26 ------ include/custom_robot.hpp | 10 ++- src/config.cpp | 15 ---- src/custom_robot.cpp | 166 ++++++++++++++++++++++++++++++++++++++- 4 files changed, 174 insertions(+), 43 deletions(-) diff --git a/include/config.hpp b/include/config.hpp index 7aa0d4b..3e52b79 100644 --- a/include/config.hpp +++ b/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(); diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index c4263b6..e02baf0 100644 --- a/include/custom_robot.hpp +++ b/include/custom_robot.hpp @@ -3,6 +3,7 @@ #include "controller.hpp" #include "config.hpp" #include "logger.hpp" +#include "mqtt.hpp" #include #include @@ -22,14 +23,21 @@ public: Controller* getController() const { return controller_.get(); } - // Robot state methods bool GetServiceList(std::vector& 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_; std::unique_ptr rsc_; + std::unique_ptr mqttClient_; CustomConfig config_; std::atomic running_; diff --git a/src/config.cpp b/src/config.cpp index 96a0ac0..20cfd5f 100644 --- a/src/config.cpp +++ b/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(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 \ No newline at end of file diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 47e4007..9d09516 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -2,13 +2,17 @@ #include #include #include +#include +#include +#include +#include +#include #include 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(); 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(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