From 99ca9456c01987270f98350a778f7d4d727c1fe9 Mon Sep 17 00:00:00 2001
From: Sucan126 <632190820@qq.com>
Date: Mon, 8 Sep 2025 20:02:14 +0800
Subject: [PATCH] Enhance MQTT connection handling and client ID generation.
 Updated the MQTT client connection method to accept username and password
 parameters, added validation for ASCII characters in username and password,
 and improved error logging for connection failures. Modified client ID
 generation to ensure it only contains valid ASCII characters, defaulting to a
 safe value if necessary.

---
 src/custom_robot.cpp | 15 ++++++++++++--
 src/mqtt.cpp         | 49 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 62 insertions(+), 2 deletions(-)

diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp
index 79fe9db..a781c7f 100644
--- a/src/custom_robot.cpp
+++ b/src/custom_robot.cpp
@@ -187,7 +187,7 @@ bool CustomRobot::initializeMqtt() {
         
         mqttClient_->startMessageProcessor();
         
-        if (!mqttClient_->connect()) {
+        if (!mqttClient_->connect(config_.mqtt_username, config_.mqtt_password)) {
             LOG_ERROR("Failed to connect to MQTT broker");
             return false;
         }
@@ -305,12 +305,23 @@ void CustomRobot::publishStatus() {
 std::string CustomRobot::generateRandomClientId() const {
     std::string baseId = config_.mqtt_client_id;
     
+    // 确保base ID只包含ASCII字符
+    std::string safeBaseId;
+    for (char c : baseId) {
+        if (std::isalnum(c) || c == '_' || c == '-') {
+            safeBaseId += c;
+        }
+    }
+    if (safeBaseId.empty()) {
+        safeBaseId = "unitree_go2_client";
+    }
+    
     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);
+    ss << safeBaseId << "_" << std::time(nullptr) << "_" << dis(gen);
     
     return ss.str();
 }
diff --git a/src/mqtt.cpp b/src/mqtt.cpp
index 2f6fd38..95bb83c 100644
--- a/src/mqtt.cpp
+++ b/src/mqtt.cpp
@@ -51,6 +51,35 @@ bool MqttClient::connect(const std::string& username, const std::string& passwor
     username_ = username;
     password_ = password;
     
+    // 验证字符串是否为有效UTF-8（实际上确保为ASCII以避免问题）
+    auto isValidMqttString = [](const std::string& str) -> bool {
+        for (unsigned char c : str) {
+            if (c > 127) { // 非ASCII字符
+                return false;
+            }
+            if (c < 32 && c != '\t' && c != '\n' && c != '\r') { // 控制字符（除了常见的空白字符）
+                return false;
+            }
+        }
+        return true;
+    };
+    
+    if (!isValidMqttString(username_)) {
+        LOG_ERROR("Username contains invalid characters");
+        return false;
+    }
+    
+    if (!isValidMqttString(password_)) {
+        LOG_ERROR("Password contains invalid characters");
+        return false;
+    }
+    
+    // 重置连接选项
+    connOpts_ = MQTTClient_connectOptions_initializer;
+    connOpts_.keepAliveInterval = 20;
+    connOpts_.cleansession = 1;
+    connOpts_.reliable = 0;
+    
     // 设置用户名和密码
     if (!username_.empty()) {
         connOpts_.username = username_.c_str();
@@ -64,6 +93,26 @@ bool MqttClient::connect(const std::string& username, const std::string& passwor
     int rc = MQTTClient_connect(client_, &connOpts_);
     if (rc != MQTTCLIENT_SUCCESS) {
         LOG_ERROR("Failed to connect to MQTT broker, return code: " + std::to_string(rc));
+        switch (rc) {
+            case 1:
+                LOG_ERROR("Connection refused: unacceptable protocol version");
+                break;
+            case 2:
+                LOG_ERROR("Connection refused: identifier rejected");
+                break;
+            case 3:
+                LOG_ERROR("Connection refused: server unavailable");
+                break;
+            case 4:
+                LOG_ERROR("Connection refused: bad user name or password (UTF-8 encoding issue)");
+                break;
+            case 5:
+                LOG_ERROR("Connection refused: not authorized");
+                break;
+            default:
+                LOG_ERROR("Connection error: code " + std::to_string(rc));
+                break;
+        }
         return false;
     }