Add command processing functionality to CustomRobot class. Introduced methods for handling OAC and Sport commands via MQTT messages, including validation and response publishing. Removed unnecessary logging statements for cleaner output.

include/custom_robot.hpp

@@ -9,6 +9,7 @@
 #include <atomic>
 #include <string>
 #include <vector>
+#include <nlohmann/json.hpp>
 #include <unitree/robot/go2/robot_state/robot_state_client.hpp>
 
 namespace custom {
@@ -27,11 +28,15 @@ public:
     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();
+    
+    void processCmd(const nlohmann::json& message);
+    bool processOacCmd(const std::string& cmd, const nlohmann::json& message);
+    bool processSportCmd(const std::string& cmd, const nlohmann::json& message);
+    void publishCmdResponse(const std::string& type, const std::string& cmd, bool success);
 
 private:
     std::string generateRandomClientId() const;

src/controller.cpp

@@ -7,7 +7,6 @@ namespace custom {
 
 Controller::Controller()
     : running_(false), initialized_(false) {
-    LOG_INFO("Controller created");
 }
 
 Controller::~Controller() {
@@ -33,7 +32,6 @@ bool Controller::initialize() {
         
         
         initialized_ = true;
-        LOG_INFO("Robot controller initialized successfully");
         return true;
         
     } catch (const std::exception& e) {
@@ -53,11 +51,8 @@ bool Controller::start() {
         return true;
     }
     
-    LOG_INFO("Starting robot controller");
-    
     running_ = true;
     
-    LOG_INFO("Robot controller started successfully");
     return true;
 }
 

src/custom_robot.cpp

@@ -71,7 +71,6 @@ bool CustomRobot::initialize() {
     }
     
     initialized_ = true;
-    LOG_INFO("CustomRobot initialized successfully");
     return true;
 }
 
@@ -121,7 +120,7 @@ bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>&
             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");
+        // LOG_INFO("Successfully retrieved service list with " + std::to_string(serviceList.size()) + " services");
         return true;
     } catch (const std::exception& e) {
         LOG_ERROR("Exception in getServiceList: " + std::string(e.what()));
@@ -192,7 +191,6 @@ bool CustomRobot::initializeMqtt() {
         
         mqttClient_->subscribe(controlTopic, 1);
         
-        LOG_INFO("MQTT client initialized successfully");
         return true;
         
     } catch (const std::exception& e) {
@@ -203,57 +201,14 @@ bool CustomRobot::initializeMqtt() {
 
 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);
+        nlohmann::json message = nlohmann::json::parse(payload);
         
-        // std::string controlTopic = config_.topic_prefix + "/" + config_.topic_cmd;
+        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();
-        //     }
-        // }
         
+        if (topic == controlTopic) {
+            processCommand(message);
+        }
     } catch (const std::exception& e) {
         LOG_ERROR("Error processing MQTT message: " + std::string(e.what()));
     }
@@ -263,38 +218,169 @@ 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() {
+
+void CustomRobot::processCmd(const nlohmann::json& message) {
+    try {
+        if (!message.contains("type") || !message.contains("cmd")) {
+            LOG_ERROR("Invalid command format: missing 'type' or 'cmd' field");
+            return;
+        }
+        
+        std::string type = message["type"];
+        std::string cmd = message["cmd"];
+        
+        LOG_INFO("Processing cmd - type: " + type + ", cmd: " + cmd);
+        
+        bool success = false;
+        
+        if (type == "oac") {
+            success = processOacCmd(cmd, message);
+        } else if (type == "sport") {
+            success = processSportCmd(cmd, message);
+        } else {
+            LOG_ERROR("Unknown command type: " + type);
+            return;
+        }
+        
+        // publishCmdResponse(type, cmd, success);
+        
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error processing cmd: " + std::string(e.what()));
+    }
+}
+
+bool CustomRobot::processOacCmd(const std::string& cmd, const nlohmann::json& message) {
+    if (!controller_) {
+        LOG_ERROR("Controller not initialized");
+        return false;
+    }
+    
+    try {
+        if (cmd == "SwitchSet") {
+            if (!message.contains("param") || !message["param"].contains("enable")) {
+                LOG_ERROR("SwitchSet cmd missing 'enable' parameter");
+                return false;
+            }
+            bool enable = message["param"]["enable"];
+            return controller_->SwitchSet(enable);
+            
+        } else if (cmd == "SwitchGet") {
+            bool enable = false;
+            bool result = controller_->SwitchGet(enable);
+            if (result) {
+                LOG_INFO("SwitchGet result: enable=" + std::string(enable ? "true" : "false"));
+            }
+            return result;
+            
+        } else if (cmd == "UseRemoteCommandFromApi") {
+            if (!message.contains("param") || !message["param"].contains("enable")) {
+                LOG_ERROR("UseRemoteCommandFromApi cmd missing 'enable' parameter");
+                return false;
+            }
+            bool enable = message["param"]["enable"];
+            return controller_->UseRemoteCommandFromApi(enable);
+            
+        } else if (cmd == "MoveToAbsolutePosition") {
+            if (!message.contains("param")) {
+                LOG_ERROR("MoveToAbsolutePosition cmd missing 'param'");
+                return false;
+            }
+            auto param = message["param"];
+            if (!param.contains("x") || !param.contains("y") || !param.contains("yaw")) {
+                LOG_ERROR("MoveToAbsolutePosition missing x, y, or yaw parameters");
+                return false;
+            }
+            float x = param["x"];
+            float y = param["y"];
+            float yaw = param["yaw"];
+            return controller_->MoveToAbsolutePosition(x, y, yaw);
+            
+        } else if (cmd == "MoveToIncrementPosition") {
+            if (!message.contains("param")) {
+                LOG_ERROR("MoveToIncrementPosition cmd missing 'param'");
+                return false;
+            }
+            auto param = message["param"];
+            if (!param.contains("x") || !param.contains("y") || !param.contains("yaw")) {
+                LOG_ERROR("MoveToIncrementPosition missing x, y, or yaw parameters");
+                return false;
+            }
+            float x = param["x"];
+            float y = param["y"];
+            float yaw = param["yaw"];
+            return controller_->MoveToIncrementPosition(x, y, yaw);
+            
+        } else {
+            LOG_ERROR("Unknown OAC command: " + cmd);
+            return false;
+        }
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error executing OAC command " + cmd + ": " + std::string(e.what()));
+        return false;
+    }
+}
+
+bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json& message) {
+    if (!controller_) {
+        LOG_ERROR("Controller not initialized");
+        return false;
+    }
+    
+    try {
+        if (cmd == "StandUp") {
+            return controller_->StandUp();
+        } else if (cmd == "StandDown") {
+            return controller_->StandDown();
+        } else if (cmd == "Sit") {
+            return controller_->Sit();
+        } else if (cmd == "Damp") {
+            return controller_->Damp();
+        } else if (cmd == "RecoveryStand") {
+            return controller_->RecoveryStand();
+        } else if (cmd == "StopMove") {
+            return controller_->StopMove();
+        } else if (cmd == "BalanceStand") {
+            return controller_->BalanceStand();
+        } else if (cmd == "Dance1") {
+            return controller_->Dance1();
+        } else if (cmd == "Dance2") {
+            return controller_->Dance2();
+        } else if (cmd == "Hello") {
+            return controller_->Hello();
+        } else {
+            LOG_ERROR("Unknown Sport command: " + cmd);
+            return false;
+        }
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error executing Sport command " + cmd + ": " + std::string(e.what()));
+        return false;
+    }
+}
+
+void CustomRobot::publishCmdResponse(const std::string& type, const std::string& cmd, bool success) {
     if (!mqttClient_ || !mqttClient_->isConnected()) {
         return;
     }
     
     try {
-        nlohmann::json status;
+        nlohmann::json response;
-        status["initialized"] = initialized_.load();
+        response["type"] = type;
-        status["running"] = running_.load();
+        response["cmd"] = cmd;
-        status["mqtt_connected"] = mqttClient_->isConnected();
+        response["success"] = success;
-        status["timestamp"] = std::time(nullptr);
+        response["timestamp"] = std::time(nullptr);
         
-        std::string statusTopic = config_.topic_prefix + "/" + config_.topic_state;
+        std::string responseTopic = config_.topic_prefix + "/response";
-        mqttClient_->publishJson(statusTopic, status, 1, true);
+        mqttClient_->publishJson(responseTopic, response, 1, false);
         
-        LOG_DEBUG("Published status to topic: " + statusTopic);
+        LOG_DEBUG("Published command response: " + response.dump());
         
     } catch (const std::exception& e) {
-        LOG_ERROR("Failed to publish status: " + std::string(e.what()));
+        LOG_ERROR("Failed to publish command response: " + std::string(e.what()));
     }
 }
 
@@ -322,4 +408,5 @@ std::string CustomRobot::generateRandomClientId() const {
     return ss.str();
 }
 
+
 } // namespace custom

src/main.cpp

@@ -35,13 +35,10 @@ int main(int argc, char** argv) {
         LOG_INFO("Robot initialized successfully");
         g_robot->start();
         
-        // Keep the program running
         LOG_INFO("System is running. Press Ctrl+C to stop.");
         
-        // Main event loop - wait for signals or other events
         while (true) {
             std::this_thread::sleep_for(std::chrono::milliseconds(100));
-            // You can add periodic tasks here if needed
         }
         
     } catch (const std::exception& e) {

src/mqtt.cpp

@@ -202,7 +202,6 @@ void MqttClient::startMessageProcessor() {
     
     processorRunning_ = true;
     messageProcessor_ = std::thread(&MqttClient::processMessageQueue, this);
-    LOG_INFO("MQTT message processor started");
 }
 
 void MqttClient::stopMessageProcessor() {