tanjianbin/lzwc-terminal-unitreeGo2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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.

Browse Source
This commit is contained in:
Sucan126
2025-09-09 11:15:03 +08:00
parent cf58ad202a
commit f46bb3b7c9
5 changed files with 162 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

225
src/custom_robot.cpp
View File

@@ -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;
// 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