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

Add RSC command processing to CustomRobot class. Introduced processRscCmd method for handling service list queries and service switching commands, enhancing functionality. Added printServiceList method for improved service status reporting. Removed unused publishCmdResponse method to streamline code.

Browse Source
This commit is contained in:
Sucan126
2025-09-09 14:25:32 +08:00
parent 54a9f9b544
commit 67b1a7fe80
2 changed files with 84 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
src/custom_robot.cpp
View File

@@ -242,12 +242,13 @@ void CustomRobot::processCmd(const nlohmann::json& message) {
success = processOacCmd(cmd, message);
} else if (type == "sport") {
success = processSportCmd(cmd, message);
} else if (type == "rsc") {
success = processRscCmd(cmd, message);
} else {
LOG_ERROR("Unknown command type: " + type);
return;
}
LOG_INFO(cmd + ", success: " + std::string(success ? "true" : "false"));
// publishCmdResponse(type, cmd, success);
} catch (const std::exception& e) {
LOG_ERROR("Error processing cmd: " + std::string(e.what()));
@@ -328,6 +329,86 @@ bool CustomRobot::processOacCmd(const std::string& cmd, const nlohmann::json& me
}
}
bool CustomRobot::processRscCmd(const std::string& cmd, const nlohmann::json& message) {
try {
if (cmd == "list") {
LOG_INFO("Received service list query command");
std::vector<unitree::robot::go2::ServiceState> serviceList;
if (!GetServiceList(serviceList)) {
LOG_ERROR("Failed to get service list");
return false;
}
LOG_INFO("Successfully retrieved service list with " + std::to_string(serviceList.size()) + " services");
int filterStatus = -1;
if (message.contains("param") && message["param"].contains("status")) {
filterStatus = message["param"]["status"];
LOG_INFO("Filtering services by status: " + std::to_string(filterStatus));
}
printServiceList(serviceList, filterStatus);
return true;
} else if (cmd == "switch") {
if (!message.contains("param") ||
!message["param"].contains("name") ||
!message["param"].contains("enable")) {
LOG_ERROR("Service switch command missing required parameters (name, enable)");
return false;
}
std::string serviceName = message["param"]["name"];
bool enable = message["param"]["enable"];
bool result = SwitchService(serviceName, enable);
if (result) {
LOG_INFO("Service switch completed successfully");
}
return result;
} else {
LOG_ERROR("Unknown service command: " + cmd);
return false;
}
} catch (const std::exception& e) {
LOG_ERROR("Error processing service cmd: " + std::string(e.what()));
return false;
}
}
void CustomRobot::printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus) {
std::string filterDesc = "All Services";
if (filterStatus == 0) {
filterDesc = "INACTIVE Services Only";
} else if (filterStatus == 1) {
filterDesc = "ACTIVE Services Only";
}
LOG_INFO("=== Service Status Report (" + filterDesc + ") ===");
int matchCount = 0;
for (const auto& service : serviceList) {
if (filterStatus != -1 && service.status != filterStatus) {
continue;
}
std::string statusStr = (service.status == 1) ? "ACTIVE" : "INACTIVE";
LOG_INFO("Service: " + service.name + " | Status: " + statusStr + " | Protect: " +
(service.protect ? "YES" : "NO"));
matchCount++;
}
if (filterStatus != -1) {
LOG_INFO("Found " + std::to_string(matchCount) + " services matching filter criteria");
}
LOG_INFO("=== End Service Report ===");
}
bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json& message) {
if (!controller_) {
LOG_ERROR("Controller not initialized");
@@ -365,28 +446,6 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
}
}
void CustomRobot::publishCmdResponse(const std::string& type, const std::string& cmd, bool success) {
if (!mqttClient_ || !mqttClient_->isConnected()) {
return;
}
try {
nlohmann::json response;
response["type"] = type;
response["cmd"] = cmd;
response["success"] = success;
response["timestamp"] = std::time(nullptr);
std::string responseTopic = config_.topic_prefix + "/response";
mqttClient_->publishJson(responseTopic, response, 1, false);
LOG_DEBUG("Published command response: " + response.dump());
} catch (const std::exception& e) {
LOG_ERROR("Failed to publish command response: " + std::string(e.what()));
}
}
std::string CustomRobot::generateRandomClientId() const {
std::string baseId = config_.mqtt_client_id;
@@ -411,5 +470,4 @@ std::string CustomRobot::generateRandomClientId() const {
return ss.str();
}
} // namespace custom