Remove MQTT dependency and refactor configuration structure. Updated CMakeLists.txt to eliminate MQTT library checks and adjusted config.hpp to use string_view for configuration parameters. Simplified controller and custom_robot classes by removing MQTT-related code and enhancing robot state management. Introduced service management methods in CustomRobot for better state handling.
This commit is contained in:
@@ -1,71 +1,48 @@
|
||||
#include "custom_robot.hpp"
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <functional>
|
||||
|
||||
namespace custom {
|
||||
|
||||
CustomRobot::CustomRobot(const std::string& configFile)
|
||||
: configFile_(configFile.empty() ? "config/robot_config.json" : configFile),
|
||||
running_(false), initialized_(false), mqttConnected_(false) {
|
||||
|
||||
stats_.startTime = std::chrono::steady_clock::now();
|
||||
}
|
||||
CustomRobot::CustomRobot()
|
||||
: initialized_(false), running_(false) {
|
||||
|
||||
}
|
||||
|
||||
CustomRobot::~CustomRobot() {
|
||||
shutdown();
|
||||
LOG_INFO("Shutting down CustomRobot");
|
||||
if (controller_) {
|
||||
controller_->shutdown();
|
||||
controller_.reset();
|
||||
}
|
||||
|
||||
if (stateClient_) {
|
||||
stateClient_.reset();
|
||||
}
|
||||
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
bool CustomRobot::initialize() {
|
||||
LOG_INFO("Initializing CustomRobot");
|
||||
|
||||
// Load configuration
|
||||
if (!Config::getInstance().loadConfig(configFile_)) {
|
||||
LOG_WARN("Failed to load config file, using defaults");
|
||||
}
|
||||
|
||||
auto& config = Config::getInstance().getConfig();
|
||||
|
||||
// Initialize robot controller
|
||||
robotController_ = std::make_unique<Controller>(config.network_interface);
|
||||
if (!robotController_->initialize()) {
|
||||
controller_ = std::make_unique<Controller>(config.network_interface);
|
||||
if (!controller_->initialize()) {
|
||||
LOG_ERROR("Failed to initialize robot controller");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set robot controller callbacks
|
||||
robotController_->setStateCallback(
|
||||
std::bind(&CustomRobot::handleRobotState, this, std::placeholders::_1)
|
||||
);
|
||||
robotController_->setErrorCallback(
|
||||
std::bind(&CustomRobot::handleRobotError, this, std::placeholders::_1)
|
||||
);
|
||||
|
||||
// Initialize MQTT client
|
||||
mqttClient_ = std::make_unique<MqttClient>(
|
||||
config.mqtt_broker, config.mqtt_port, config.mqtt_client_id
|
||||
);
|
||||
|
||||
// Set MQTT callbacks
|
||||
mqttClient_->setMessageCallback(
|
||||
std::bind(&CustomRobot::handleMqttMessage, this, std::placeholders::_1, std::placeholders::_2)
|
||||
);
|
||||
mqttClient_->setConnectionCallback(
|
||||
std::bind(&CustomRobot::handleMqttConnection, this, std::placeholders::_1)
|
||||
);
|
||||
|
||||
// Connect to MQTT broker
|
||||
if (!mqttClient_->connect(config.mqtt_username, config.mqtt_password)) {
|
||||
LOG_ERROR("Failed to connect to MQTT broker");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Start MQTT message processor
|
||||
mqttClient_->startMessageProcessor();
|
||||
|
||||
// Subscribe to command topics
|
||||
std::string cmdTopic = Config::getInstance().getFullTopic(config.cmd_topic);
|
||||
if (!mqttClient_->subscribe(cmdTopic + "/+")) { // Subscribe to all command subtopics
|
||||
LOG_ERROR("Failed to subscribe to command topics");
|
||||
// Initialize robot state client
|
||||
try {
|
||||
stateClient_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
|
||||
stateClient_->SetTimeout(10.0f);
|
||||
stateClient_->Init();
|
||||
LOG_INFO("RobotStateClient initialized successfully");
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Failed to initialize RobotStateClient: " + std::string(e.what()));
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -74,24 +51,6 @@ bool CustomRobot::initialize() {
|
||||
return true;
|
||||
}
|
||||
|
||||
void CustomRobot::shutdown() {
|
||||
LOG_INFO("Shutting down CustomRobot");
|
||||
|
||||
stop();
|
||||
|
||||
if (mqttClient_) {
|
||||
mqttClient_->stopMessageProcessor();
|
||||
mqttClient_->disconnect();
|
||||
mqttClient_.reset();
|
||||
}
|
||||
|
||||
if (robotController_) {
|
||||
robotController_->shutdown();
|
||||
robotController_.reset();
|
||||
}
|
||||
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
bool CustomRobot::start() {
|
||||
if (!initialized_) {
|
||||
@@ -104,579 +63,74 @@ bool CustomRobot::start() {
|
||||
return true;
|
||||
}
|
||||
|
||||
LOG_INFO("Starting CustomRobot");
|
||||
|
||||
// Start robot controller
|
||||
if (!robotController_->start()) {
|
||||
if (!controller_->start()) {
|
||||
LOG_ERROR("Failed to start robot controller");
|
||||
return false;
|
||||
}
|
||||
|
||||
running_ = true;
|
||||
|
||||
// Start periodic tasks
|
||||
startPeriodicTasks();
|
||||
|
||||
// Publish initial status
|
||||
publishHeartbeat();
|
||||
|
||||
LOG_INFO("CustomRobot started successfully");
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CustomRobot::stop() {
|
||||
if (!running_) {
|
||||
bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
|
||||
if (!initialized_ || !stateClient_) {
|
||||
LOG_ERROR("Robot not initialized or stateClient is null");
|
||||
return false;
|
||||
}
|
||||
|
||||
try {
|
||||
int32_t ret = stateClient_->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;
|
||||
}
|
||||
|
||||
LOG_INFO("Stopping CustomRobot");
|
||||
|
||||
running_ = false;
|
||||
|
||||
// Stop periodic tasks
|
||||
stopPeriodicTasks();
|
||||
|
||||
// Stop robot controller
|
||||
if (robotController_) {
|
||||
robotController_->stop();
|
||||
}
|
||||
|
||||
// Publish final status
|
||||
if (mqttClient_ && mqttClient_->isConnected()) {
|
||||
nlohmann::json status;
|
||||
status["status"] = "stopped";
|
||||
status["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
|
||||
auto& config = Config::getInstance().getConfig();
|
||||
std::string statusTopic = Config::getInstance().getFullTopic(config.state_topic + "/status");
|
||||
mqttClient_->publishJson(statusTopic, status);
|
||||
}
|
||||
|
||||
LOG_INFO("CustomRobot stopped");
|
||||
return true;
|
||||
}
|
||||
|
||||
void CustomRobot::run() {
|
||||
if (!start()) {
|
||||
LOG_ERROR("Failed to start CustomRobot");
|
||||
return;
|
||||
}
|
||||
|
||||
LOG_INFO("CustomRobot main loop started");
|
||||
|
||||
while (running_) {
|
||||
try {
|
||||
// Process any pending errors
|
||||
processErrorQueue();
|
||||
|
||||
// Sleep for a short period
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Main loop error: " + std::string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
LOG_INFO("CustomRobot main loop ended");
|
||||
}
|
||||
|
||||
void CustomRobot::runAsync() {
|
||||
mainThread_ = std::thread(&CustomRobot::run, this);
|
||||
}
|
||||
|
||||
void CustomRobot::waitForShutdown() {
|
||||
if (mainThread_.joinable()) {
|
||||
mainThread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::handleMqttMessage(const std::string& topic, const std::string& payload) {
|
||||
try {
|
||||
LOG_DEBUG("Received MQTT message on topic: " + topic);
|
||||
|
||||
auto& config = Config::getInstance().getConfig();
|
||||
std::string cmdPrefix = Config::getInstance().getFullTopic(config.cmd_topic);
|
||||
|
||||
if (topic.find(cmdPrefix) == 0) {
|
||||
// Parse JSON payload
|
||||
nlohmann::json message = nlohmann::json::parse(payload);
|
||||
|
||||
// Extract command type from topic
|
||||
std::string cmdType = topic.substr(cmdPrefix.length() + 1);
|
||||
|
||||
if (cmdType == "motion") {
|
||||
handleCommandMessage(message);
|
||||
} else if (cmdType == "config") {
|
||||
handleConfigMessage(message);
|
||||
} else if (cmdType == "control") {
|
||||
handleControlMessage(message);
|
||||
} else {
|
||||
LOG_WARN("Unknown command type: " + cmdType);
|
||||
}
|
||||
|
||||
stats_.commandsReceived++;
|
||||
}
|
||||
|
||||
} catch (const nlohmann::json::parse_error& e) {
|
||||
LOG_ERROR("JSON parse error: " + std::string(e.what()));
|
||||
publishError("Invalid JSON in message: " + std::string(e.what()));
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Message handling error: " + std::string(e.what()));
|
||||
publishError("Message handling error: " + std::string(e.what()));
|
||||
LOG_ERROR("Exception in getServiceList: " + std::string(e.what()));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::handleCommandMessage(const nlohmann::json& command) {
|
||||
std::string requestId = command.value("request_id", "");
|
||||
|
||||
try {
|
||||
if (!command.contains("type")) {
|
||||
publishResponse(requestId, false, "Missing command type");
|
||||
return;
|
||||
}
|
||||
|
||||
std::string cmdType = command["type"];
|
||||
bool success = false;
|
||||
|
||||
if (cmdType == "motion") {
|
||||
success = processMotionCommand(command);
|
||||
} else if (cmdType == "special_action") {
|
||||
success = processSpecialAction(command);
|
||||
} else if (cmdType == "system") {
|
||||
success = processSystemCommand(command);
|
||||
} else {
|
||||
publishResponse(requestId, false, "Unknown command type: " + cmdType);
|
||||
return;
|
||||
}
|
||||
|
||||
if (success) {
|
||||
stats_.commandsExecuted++;
|
||||
publishResponse(requestId, true);
|
||||
} else {
|
||||
stats_.commandsFailed++;
|
||||
publishResponse(requestId, false, "Command execution failed");
|
||||
}
|
||||
|
||||
lastCommandReceived_ = std::chrono::steady_clock::now();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Command processing error: " + std::string(e.what()));
|
||||
publishResponse(requestId, false, "Command processing error: " + std::string(e.what()));
|
||||
stats_.commandsFailed++;
|
||||
}
|
||||
}
|
||||
|
||||
bool CustomRobot::processMotionCommand(const nlohmann::json& cmd) {
|
||||
if (!robotController_) {
|
||||
bool CustomRobot::SwitchService(const std::string& serviceName, bool enable) {
|
||||
if (!initialized_ || !stateClient_) {
|
||||
LOG_ERROR("Robot not initialized or stateClient is null");
|
||||
return false;
|
||||
}
|
||||
|
||||
MotionCommand motionCmd = jsonToMotionCommand(cmd);
|
||||
return robotController_->executeCommand(motionCmd);
|
||||
try {
|
||||
int32_t status;
|
||||
int32_t ret = stateClient_->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::processSpecialAction(const nlohmann::json& cmd) {
|
||||
if (!robotController_ || !cmd.contains("action")) {
|
||||
bool CustomRobot::SetReportFreq(int32_t interval, int32_t duration) {
|
||||
if (!initialized_ || !stateClient_) {
|
||||
LOG_ERROR("Robot not initialized or stateClient is null");
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string action = cmd["action"];
|
||||
nlohmann::json params = cmd.value("params", nlohmann::json::object());
|
||||
|
||||
return robotController_->performAction(action, params);
|
||||
}
|
||||
|
||||
bool CustomRobot::processSystemCommand(const nlohmann::json& cmd) {
|
||||
if (!cmd.contains("command")) {
|
||||
try {
|
||||
int32_t ret = stateClient_->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;
|
||||
}
|
||||
|
||||
std::string sysCmd = cmd["command"];
|
||||
|
||||
if (sysCmd == "emergency_stop") {
|
||||
return robotController_ && robotController_->emergencyStop();
|
||||
} else if (sysCmd == "recovery_stand") {
|
||||
return robotController_ && robotController_->recoveryStand();
|
||||
} else if (sysCmd == "start") {
|
||||
return robotController_ && robotController_->start();
|
||||
} else if (sysCmd == "stop") {
|
||||
return robotController_ && robotController_->stop();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void CustomRobot::handleConfigMessage(const nlohmann::json& config) {
|
||||
// Handle configuration updates
|
||||
LOG_INFO("Received configuration update");
|
||||
|
||||
try {
|
||||
// Update configuration and save
|
||||
auto& currentConfig = Config::getInstance().getConfig();
|
||||
|
||||
if (config.contains("control_frequency")) {
|
||||
currentConfig.control_frequency = config["control_frequency"];
|
||||
}
|
||||
if (config.contains("state_publish_frequency")) {
|
||||
currentConfig.state_publish_frequency = config["state_publish_frequency"];
|
||||
}
|
||||
|
||||
// Save updated configuration
|
||||
Config::getInstance().saveConfig(configFile_);
|
||||
|
||||
LOG_INFO("Configuration updated successfully");
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Configuration update error: " + std::string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::handleControlMessage(const nlohmann::json& control) {
|
||||
// Handle control messages (start/stop/pause etc.)
|
||||
if (control.contains("command")) {
|
||||
std::string cmd = control["command"];
|
||||
|
||||
if (cmd == "start" && !running_) {
|
||||
start();
|
||||
} else if (cmd == "stop" && running_) {
|
||||
stop();
|
||||
} else if (cmd == "restart") {
|
||||
stop();
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
start();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::handleRobotState(const RobotState& state) {
|
||||
// Cache the latest state
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(stateCacheMutex_);
|
||||
lastKnownState_ = state;
|
||||
}
|
||||
|
||||
// Publish state if enough time has passed
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto timeSinceLastPublish = std::chrono::duration<double>(now - lastStatePublish_).count();
|
||||
|
||||
if (timeSinceLastPublish >= statePublishInterval_) {
|
||||
publishState();
|
||||
lastStatePublish_ = now;
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::handleRobotError(const std::string& error) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(errorMutex_);
|
||||
errorQueue_.push(error);
|
||||
}
|
||||
|
||||
stats_.errorsOccurred++;
|
||||
}
|
||||
|
||||
void CustomRobot::publishState() {
|
||||
if (!mqttClient_ || !mqttClient_->isConnected()) {
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
RobotState state;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(stateCacheMutex_);
|
||||
state = lastKnownState_;
|
||||
}
|
||||
|
||||
nlohmann::json stateJson = robotStateToJson(state);
|
||||
|
||||
auto& config = Config::getInstance().getConfig();
|
||||
std::string stateTopic = Config::getInstance().getFullTopic(config.state_topic + "/robot");
|
||||
|
||||
mqttClient_->publishJson(stateTopic, stateJson);
|
||||
stats_.statesPublished++;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("State publishing error: " + std::string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::publishHeartbeat() {
|
||||
if (!mqttClient_ || !mqttClient_->isConnected()) {
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
nlohmann::json heartbeat = createStatusMessage();
|
||||
|
||||
auto& config = Config::getInstance().getConfig();
|
||||
std::string heartbeatTopic = Config::getInstance().getFullTopic(config.state_topic + "/heartbeat");
|
||||
|
||||
mqttClient_->publishJson(heartbeatTopic, heartbeat);
|
||||
lastHeartbeat_ = std::chrono::steady_clock::now();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Heartbeat publishing error: " + std::string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::publishError(const std::string& error) {
|
||||
if (!mqttClient_ || !mqttClient_->isConnected()) {
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
nlohmann::json errorMsg;
|
||||
errorMsg["error"] = error;
|
||||
errorMsg["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
|
||||
auto& config = Config::getInstance().getConfig();
|
||||
std::string errorTopic = Config::getInstance().getFullTopic(config.state_topic + "/error");
|
||||
|
||||
mqttClient_->publishJson(errorTopic, errorMsg);
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Error publishing error: " + std::string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::publishResponse(const std::string& requestId, bool success, const std::string& message) {
|
||||
if (!mqttClient_ || !mqttClient_->isConnected() || requestId.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
nlohmann::json response;
|
||||
response["request_id"] = requestId;
|
||||
response["success"] = success;
|
||||
response["message"] = message;
|
||||
response["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
|
||||
auto& config = Config::getInstance().getConfig();
|
||||
std::string responseTopic = Config::getInstance().getFullTopic(config.state_topic + "/response");
|
||||
|
||||
mqttClient_->publishJson(responseTopic, response);
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Response publishing error: " + std::string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
MotionCommand CustomRobot::jsonToMotionCommand(const nlohmann::json& json) {
|
||||
MotionCommand cmd;
|
||||
cmd.timestamp = std::chrono::steady_clock::now();
|
||||
|
||||
if (json.contains("motion_type")) {
|
||||
std::string motionType = json["motion_type"];
|
||||
if (motionType == "velocity") {
|
||||
cmd.type = MotionCommand::VELOCITY;
|
||||
} else if (motionType == "position") {
|
||||
cmd.type = MotionCommand::POSITION;
|
||||
} else if (motionType == "body_pose") {
|
||||
cmd.type = MotionCommand::BODY_POSE;
|
||||
} else if (motionType == "special_action") {
|
||||
cmd.type = MotionCommand::SPECIAL_ACTION;
|
||||
}
|
||||
}
|
||||
|
||||
// Parse velocity command
|
||||
if (json.contains("linear_velocity")) {
|
||||
auto vel = json["linear_velocity"];
|
||||
if (vel.is_array() && vel.size() >= 3) {
|
||||
cmd.linear_velocity[0] = vel[0];
|
||||
cmd.linear_velocity[1] = vel[1];
|
||||
cmd.linear_velocity[2] = vel[2];
|
||||
}
|
||||
}
|
||||
|
||||
if (json.contains("angular_velocity")) {
|
||||
auto vel = json["angular_velocity"];
|
||||
if (vel.is_array() && vel.size() >= 3) {
|
||||
cmd.angular_velocity[0] = vel[0];
|
||||
cmd.angular_velocity[1] = vel[1];
|
||||
cmd.angular_velocity[2] = vel[2];
|
||||
}
|
||||
}
|
||||
|
||||
// Parse position command
|
||||
if (json.contains("target_position")) {
|
||||
auto pos = json["target_position"];
|
||||
if (pos.is_array() && pos.size() >= 3) {
|
||||
cmd.target_position[0] = pos[0];
|
||||
cmd.target_position[1] = pos[1];
|
||||
cmd.target_position[2] = pos[2];
|
||||
}
|
||||
}
|
||||
|
||||
// Parse body pose
|
||||
if (json.contains("body_height")) cmd.body_height = json["body_height"];
|
||||
if (json.contains("body_roll")) cmd.body_roll = json["body_roll"];
|
||||
if (json.contains("body_pitch")) cmd.body_pitch = json["body_pitch"];
|
||||
if (json.contains("body_yaw")) cmd.body_yaw = json["body_yaw"];
|
||||
|
||||
// Parse special action
|
||||
if (json.contains("action_name")) {
|
||||
cmd.action_name = json["action_name"];
|
||||
}
|
||||
if (json.contains("action_params")) {
|
||||
cmd.action_params = json["action_params"];
|
||||
}
|
||||
|
||||
// Parse duration
|
||||
if (json.contains("duration")) {
|
||||
cmd.duration = json["duration"];
|
||||
}
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
nlohmann::json CustomRobot::robotStateToJson(const RobotState& state) {
|
||||
nlohmann::json json;
|
||||
|
||||
// Basic info
|
||||
json["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
state.timestamp.time_since_epoch()).count();
|
||||
json["is_connected"] = state.is_connected;
|
||||
json["emergency_stop"] = state.emergency_stop;
|
||||
|
||||
// Position and orientation
|
||||
json["position"] = {state.position[0], state.position[1], state.position[2]};
|
||||
json["orientation"] = {state.orientation[0], state.orientation[1], state.orientation[2]};
|
||||
json["velocity"] = {state.velocity[0], state.velocity[1], state.velocity[2]};
|
||||
|
||||
// IMU data
|
||||
json["imu"]["acceleration"] = {state.imu_acc[0], state.imu_acc[1], state.imu_acc[2]};
|
||||
json["imu"]["gyroscope"] = {state.imu_gyro[0], state.imu_gyro[1], state.imu_gyro[2]};
|
||||
json["imu"]["quaternion"] = {state.imu_quat[0], state.imu_quat[1], state.imu_quat[2], state.imu_quat[3]};
|
||||
|
||||
// Battery info
|
||||
json["battery"]["voltage"] = state.battery_voltage;
|
||||
json["battery"]["current"] = state.battery_current;
|
||||
json["battery"]["percentage"] = state.battery_percentage;
|
||||
json["battery"]["low_battery"] = state.low_battery;
|
||||
|
||||
// System info
|
||||
json["temperature"] = state.temperature;
|
||||
json["overheated"] = state.overheated;
|
||||
|
||||
// Motor states (first 12 leg motors)
|
||||
json["motors"]["positions"] = std::vector<double>(state.motor_positions, state.motor_positions + 12);
|
||||
json["motors"]["velocities"] = std::vector<double>(state.motor_velocities, state.motor_velocities + 12);
|
||||
json["motors"]["torques"] = std::vector<double>(state.motor_torques, state.motor_torques + 12);
|
||||
json["motors"]["temperatures"] = std::vector<double>(state.motor_temperatures, state.motor_temperatures + 12);
|
||||
|
||||
return json;
|
||||
}
|
||||
|
||||
nlohmann::json CustomRobot::createStatusMessage() {
|
||||
nlohmann::json status;
|
||||
|
||||
status["status"] = running_ ? "running" : "stopped";
|
||||
status["initialized"] = initialized_;
|
||||
status["mqtt_connected"] = mqttConnected_;
|
||||
status["robot_connected"] = robotController_ && robotController_->isRunning();
|
||||
|
||||
// Statistics
|
||||
status["stats"]["commands_received"] = stats_.commandsReceived;
|
||||
status["stats"]["commands_executed"] = stats_.commandsExecuted;
|
||||
status["stats"]["commands_failed"] = stats_.commandsFailed;
|
||||
status["stats"]["states_published"] = stats_.statesPublished;
|
||||
status["stats"]["errors_occurred"] = stats_.errorsOccurred;
|
||||
|
||||
// Uptime
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto uptime = std::chrono::duration_cast<std::chrono::seconds>(now - stats_.startTime).count();
|
||||
status["uptime_seconds"] = uptime;
|
||||
|
||||
status["timestamp"] = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
void CustomRobot::startPeriodicTasks() {
|
||||
// Heartbeat thread
|
||||
periodicThreads_.emplace_back([this]() {
|
||||
while (running_) {
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto timeSinceLastHeartbeat = std::chrono::duration<double>(now - lastHeartbeat_).count();
|
||||
|
||||
if (timeSinceLastHeartbeat >= heartbeatInterval_) {
|
||||
publishHeartbeat();
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
});
|
||||
|
||||
// Safety monitor thread
|
||||
safetyThread_ = std::thread(&CustomRobot::safetyMonitorLoop, this);
|
||||
}
|
||||
|
||||
void CustomRobot::stopPeriodicTasks() {
|
||||
// Wait for all periodic threads to finish
|
||||
for (auto& thread : periodicThreads_) {
|
||||
if (thread.joinable()) {
|
||||
thread.join();
|
||||
}
|
||||
}
|
||||
periodicThreads_.clear();
|
||||
|
||||
if (safetyThread_.joinable()) {
|
||||
safetyThread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::handleMqttConnection(bool connected) {
|
||||
mqttConnected_ = connected;
|
||||
|
||||
if (connected) {
|
||||
LOG_INFO("MQTT connection established");
|
||||
|
||||
// Resubscribe to topics
|
||||
auto& config = Config::getInstance().getConfig();
|
||||
std::string cmdTopic = Config::getInstance().getFullTopic(config.cmd_topic);
|
||||
mqttClient_->subscribe(cmdTopic + "/+");
|
||||
|
||||
// Publish connection status
|
||||
publishHeartbeat();
|
||||
} else {
|
||||
LOG_WARN("MQTT connection lost");
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::safetyMonitorLoop() {
|
||||
while (running_) {
|
||||
try {
|
||||
checkEmergencyConditions();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
} catch (const std::exception& e) {
|
||||
LOG_ERROR("Safety monitor error: " + std::string(e.what()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::checkEmergencyConditions() {
|
||||
// Check command timeout
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto timeSinceLastCommand = std::chrono::duration<double>(now - lastCommandReceived_).count();
|
||||
|
||||
if (timeSinceLastCommand > maxIdleTime_ && robotController_ && robotController_->isRunning()) {
|
||||
LOG_WARN("Maximum idle time exceeded, stopping robot");
|
||||
robotController_->emergencyStop();
|
||||
}
|
||||
}
|
||||
|
||||
void CustomRobot::processErrorQueue() {
|
||||
std::lock_guard<std::mutex> lock(errorMutex_);
|
||||
|
||||
while (!errorQueue_.empty()) {
|
||||
std::string error = errorQueue_.front();
|
||||
errorQueue_.pop();
|
||||
|
||||
publishError(error);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace custom
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user