From f46bb3b7c97af2e15de4f06efc2bd877678665bb Mon Sep 17 00:00:00 2001 From: Sucan126 <632190820@qq.com> Date: Tue, 9 Sep 2025 11:15:03 +0800 Subject: [PATCH] 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 | 7 +- src/controller.cpp | 5 - src/custom_robot.cpp | 225 +++++++++++++++++++++++++++------------ src/main.cpp | 3 - src/mqtt.cpp | 1 - 5 files changed, 162 insertions(+), 79 deletions(-) diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index e02baf0..2c062ef 100644 --- a/include/custom_robot.hpp +++ b/include/custom_robot.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include 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; diff --git a/src/controller.cpp b/src/controller.cpp index edbb329..af52c6a 100644 --- a/src/controller.cpp +++ b/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; } diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 49c0330..eba76c0 100644 --- a/src/custom_robot.cpp +++ b/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& 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; - - // 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(); - // } - // } + std::string controlTopic = config_.topic_prefix + "/" + config_.topic_cmd; + 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; - status["initialized"] = initialized_.load(); - status["running"] = running_.load(); - status["mqtt_connected"] = mqttClient_->isConnected(); - status["timestamp"] = std::time(nullptr); + nlohmann::json response; + response["type"] = type; + response["cmd"] = cmd; + response["success"] = success; + response["timestamp"] = std::time(nullptr); - std::string statusTopic = config_.topic_prefix + "/" + config_.topic_state; - mqttClient_->publishJson(statusTopic, status, 1, true); + std::string responseTopic = config_.topic_prefix + "/response"; + 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 diff --git a/src/main.cpp b/src/main.cpp index cc2f2da..55e538e 100644 --- a/src/main.cpp +++ b/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) { diff --git a/src/mqtt.cpp b/src/mqtt.cpp index 3fa774d..bc8487e 100644 --- a/src/mqtt.cpp +++ b/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() {