124 lines
3.4 KiB
C++
124 lines
3.4 KiB
C++
#pragma once
|
|
|
|
#include "controller.hpp"
|
|
#include "mqtt.hpp"
|
|
#include "config.hpp"
|
|
#include "logger.hpp"
|
|
|
|
#include <memory>
|
|
#include <thread>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <nlohmann/json.hpp>
|
|
|
|
namespace custom {
|
|
|
|
class CustomRobot {
|
|
public:
|
|
explicit CustomRobot(const std::string& configFile = "");
|
|
~CustomRobot();
|
|
|
|
bool initialize();
|
|
void shutdown();
|
|
|
|
bool start();
|
|
bool stop();
|
|
bool isRunning() const { return running_; }
|
|
|
|
// Main run loop
|
|
void run();
|
|
void runAsync();
|
|
void waitForShutdown();
|
|
|
|
private:
|
|
// MQTT message handlers
|
|
void handleMqttMessage(const std::string& topic, const std::string& payload);
|
|
void handleCommandMessage(const nlohmann::json& command);
|
|
void handleConfigMessage(const nlohmann::json& config);
|
|
void handleControlMessage(const nlohmann::json& control);
|
|
|
|
// Robot state handlers
|
|
void handleRobotState(const RobotState& state);
|
|
void handleRobotError(const std::string& error);
|
|
|
|
// Publishing functions
|
|
void publishState();
|
|
void publishHeartbeat();
|
|
void publishError(const std::string& error);
|
|
void publishResponse(const std::string& requestId, bool success, const std::string& message = "");
|
|
|
|
// Command processing
|
|
bool processMotionCommand(const nlohmann::json& cmd);
|
|
bool processSpecialAction(const nlohmann::json& cmd);
|
|
bool processSystemCommand(const nlohmann::json& cmd);
|
|
bool processConfigCommand(const nlohmann::json& cmd);
|
|
|
|
// Utility functions
|
|
MotionCommand jsonToMotionCommand(const nlohmann::json& json);
|
|
nlohmann::json robotStateToJson(const RobotState& state);
|
|
nlohmann::json createStatusMessage();
|
|
|
|
void startPeriodicTasks();
|
|
void stopPeriodicTasks();
|
|
|
|
// Connection management
|
|
void handleMqttConnection(bool connected);
|
|
void reconnectMqtt();
|
|
|
|
// Safety monitoring
|
|
void safetyMonitorLoop();
|
|
void checkEmergencyConditions();
|
|
|
|
// Components
|
|
std::unique_ptr<Controller> robotController_;
|
|
std::unique_ptr<MqttClient> mqttClient_;
|
|
|
|
// Configuration
|
|
std::string configFile_;
|
|
|
|
// State
|
|
std::atomic<bool> running_;
|
|
std::atomic<bool> initialized_;
|
|
std::atomic<bool> mqttConnected_;
|
|
|
|
// Threads
|
|
std::thread mainThread_;
|
|
std::thread safetyThread_;
|
|
std::vector<std::thread> periodicThreads_;
|
|
|
|
// Timing
|
|
std::chrono::steady_clock::time_point lastHeartbeat_;
|
|
std::chrono::steady_clock::time_point lastStatePublish_;
|
|
std::chrono::steady_clock::time_point lastCommandReceived_;
|
|
|
|
// Statistics
|
|
struct Statistics {
|
|
uint64_t commandsReceived = 0;
|
|
uint64_t commandsExecuted = 0;
|
|
uint64_t commandsFailed = 0;
|
|
uint64_t statesPublished = 0;
|
|
uint64_t errorsOccurred = 0;
|
|
std::chrono::steady_clock::time_point startTime;
|
|
} stats_;
|
|
|
|
// Safety parameters
|
|
double maxIdleTime_ = 30.0; // seconds
|
|
double heartbeatInterval_ = 5.0; // seconds
|
|
double statePublishInterval_ = 0.02; // 50Hz
|
|
|
|
// Current robot state cache
|
|
mutable std::mutex stateCacheMutex_;
|
|
RobotState lastKnownState_;
|
|
|
|
// Request tracking
|
|
std::mutex requestMutex_;
|
|
std::map<std::string, std::chrono::steady_clock::time_point> pendingRequests_;
|
|
|
|
// Error handling
|
|
std::mutex errorMutex_;
|
|
std::queue<std::string> errorQueue_;
|
|
void processErrorQueue();
|
|
};
|
|
|
|
} // namespace custom
|