330 lines
11 KiB
C++
330 lines
11 KiB
C++
#include "custom_robot.hpp"
|
|
#include <thread>
|
|
#include <chrono>
|
|
#include <functional>
|
|
#include <ctime>
|
|
#include <random>
|
|
#include <sstream>
|
|
#include <iomanip>
|
|
#include <nlohmann/json.hpp>
|
|
#include <unitree/robot/channel/channel_factory.hpp>
|
|
|
|
namespace custom {
|
|
|
|
CustomRobot::CustomRobot()
|
|
: initialized_(false), running_(false) {
|
|
config_.loadDefaults();
|
|
|
|
try {
|
|
unitree::robot::ChannelFactory::Instance()->Init(0, config_.network_interface);
|
|
LOG_INFO("ChannelFactory initialized with interface: " + config_.network_interface);
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Failed to initialize ChannelFactory: " + std::string(e.what()));
|
|
}
|
|
}
|
|
|
|
CustomRobot::~CustomRobot() {
|
|
LOG_INFO("Shutting down CustomRobot");
|
|
|
|
if (mqttClient_) {
|
|
mqttClient_->stopMessageProcessor();
|
|
mqttClient_->disconnect();
|
|
mqttClient_.reset();
|
|
LOG_INFO("MQTT client disconnected and cleaned up");
|
|
}
|
|
|
|
if (controller_) {
|
|
controller_->shutdown();
|
|
controller_.reset();
|
|
}
|
|
|
|
if (rsc_) {
|
|
rsc_.reset();
|
|
}
|
|
|
|
try {
|
|
unitree::robot::ChannelFactory::Instance()->Release();
|
|
LOG_INFO("ChannelFactory released");
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Failed to release ChannelFactory: " + std::string(e.what()));
|
|
}
|
|
|
|
initialized_ = false;
|
|
}
|
|
|
|
bool CustomRobot::initialize() {
|
|
try {
|
|
controller_ = std::make_unique<Controller>();
|
|
controller_->initialize();
|
|
|
|
rsc_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
|
|
rsc_->SetTimeout(3.0f);
|
|
rsc_->Init();
|
|
|
|
if (!initializeMqtt()) {
|
|
LOG_ERROR("Failed to initialize MQTT client");
|
|
return false;
|
|
}
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Failed to initialize : " + std::string(e.what()));
|
|
return false;
|
|
}
|
|
|
|
initialized_ = true;
|
|
LOG_INFO("CustomRobot initialized successfully");
|
|
return true;
|
|
}
|
|
|
|
|
|
bool CustomRobot::start() {
|
|
if (!initialized_) {
|
|
LOG_ERROR("Cannot start: not initialized");
|
|
return false;
|
|
}
|
|
|
|
if (running_) {
|
|
LOG_WARN("CustomRobot already running");
|
|
return true;
|
|
}
|
|
|
|
std::vector<unitree::robot::go2::ServiceState> serviceList;
|
|
if (!GetServiceList(serviceList)) {
|
|
LOG_ERROR("Failed to get service list");
|
|
return false;
|
|
}
|
|
|
|
for (const auto& service : serviceList) {
|
|
LOG_INFO("Service - name: " + service.name + ", protect: " + std::to_string(service.protect) + ", status: " + std::to_string(service.status));
|
|
}
|
|
|
|
if (!SwitchService("sport_mode", false)) {
|
|
LOG_ERROR("Failed to stop sport_mode service");
|
|
return false;
|
|
}
|
|
|
|
std::this_thread::sleep_for(std::chrono::seconds(1));
|
|
|
|
if (!controller_->start()) {
|
|
LOG_ERROR("Failed to start robot controller");
|
|
return false;
|
|
}
|
|
running_ = true;
|
|
LOG_INFO("CustomRobot started successfully");
|
|
return true;
|
|
}
|
|
|
|
bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
|
|
if (!initialized_ || !rsc_) {
|
|
LOG_ERROR("Robot not initialized or stateClient is null");
|
|
return false;
|
|
}
|
|
|
|
try {
|
|
int32_t ret = rsc_->ServiceList(serviceList);
|
|
if (ret != 0) {
|
|
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");
|
|
return true;
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Exception in getServiceList: " + std::string(e.what()));
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
|
|
if (!initialized_ || !rsc_) {
|
|
LOG_ERROR("Robot not initialized or stateClient is null");
|
|
return false;
|
|
}
|
|
|
|
try {
|
|
int32_t status;
|
|
int32_t ret = rsc_->ServiceSwitch(serviceName, enable ? 1 : 0, status);
|
|
if (ret != 0) {
|
|
LOG_ERROR("Failed to switch service " + serviceName + ", error code: " + std::to_string(ret));
|
|
return false;
|
|
}
|
|
LOG_INFO("Successfully switched service " + serviceName + " to " + (enable ? "enabled" : "disabled"));
|
|
return true;
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Exception in switchService: " + std::string(e.what()));
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
|
|
if (!initialized_ || !rsc_) {
|
|
LOG_ERROR("Robot not initialized or stateClient is null");
|
|
return false;
|
|
}
|
|
|
|
try {
|
|
int32_t ret = rsc_->SetReportFreq(interval, duration);
|
|
if (ret != 0) {
|
|
LOG_ERROR("Failed to set report frequency, error code: " + std::to_string(ret));
|
|
return false;
|
|
}
|
|
LOG_INFO("Successfully set report frequency: interval=" + std::to_string(interval) + ", duration=" + std::to_string(duration));
|
|
return true;
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Exception in setReportFreq: " + std::string(e.what()));
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool CustomRobot::initializeMqtt() {
|
|
try {
|
|
std::string broker = config_.mqtt_broker;
|
|
int port = config_.mqtt_port;
|
|
std::string clientId = generateRandomClientId();
|
|
|
|
mqttClient_ = std::make_unique<MqttClient>(broker, port, clientId);
|
|
using namespace std::placeholders;
|
|
mqttClient_->setMessageCallback(std::bind(&CustomRobot::onMqttMessage, this, _1, _2));
|
|
mqttClient_->setConnectionCallback(std::bind(&CustomRobot::onMqttConnection, this, _1));
|
|
|
|
mqttClient_->startMessageProcessor();
|
|
|
|
if (!mqttClient_->connect(config_.mqtt_username, config_.mqtt_password)) {
|
|
LOG_ERROR("Failed to connect to MQTT broker");
|
|
return false;
|
|
}
|
|
|
|
std::string controlTopic = config_.topic_prefix + "/" + config_.topic_cmd;
|
|
|
|
mqttClient_->subscribe(controlTopic, 1);
|
|
|
|
LOG_INFO("MQTT client initialized successfully");
|
|
return true;
|
|
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Exception during MQTT initialization: " + std::string(e.what()));
|
|
return false;
|
|
}
|
|
}
|
|
|
|
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);
|
|
|
|
// 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();
|
|
// }
|
|
// }
|
|
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Error processing MQTT message: " + std::string(e.what()));
|
|
}
|
|
}
|
|
|
|
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() {
|
|
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);
|
|
|
|
std::string statusTopic = config_.topic_prefix + "/" + config_.topic_state;
|
|
mqttClient_->publishJson(statusTopic, status, 1, true);
|
|
|
|
LOG_DEBUG("Published status to topic: " + statusTopic);
|
|
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("Failed to publish status: " + std::string(e.what()));
|
|
}
|
|
}
|
|
|
|
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 << safeBaseId << "_" << std::time(nullptr) << "_" << dis(gen);
|
|
|
|
return ss.str();
|
|
}
|
|
|
|
} // namespace custom
|