diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index f378760..e83c870 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -1,11 +1,10 @@ -#include "KobukiDriver/CKobuki.h" -#include "MQTT/MqttClient.h" -#include #include -#include -#include #include - +#include +#include "MQTT/MqttClient.h" +#include "KobukiDriver/CKobuki.h" +#include +#include using namespace std; using namespace cv; @@ -17,13 +16,13 @@ void parseMQTT(std::string message); void CapnSend(); void checkKobukiConnection(); // ip, clientID, username, password -MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", - "rpiwachtwoordofzo"); // create a client object +MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object std::string message = "stop"; std::string serializeKobukiData(const TKobukiData &data); void sendKobukiData(TKobukiData &data); -void setup() { +void setup() +{ unsigned char *null_ptr(0); robot.startCommunication("/dev/ttyUSB0", true, null_ptr); // connect mqtt server and sub to commands @@ -31,17 +30,21 @@ void setup() { client.subscribe("home/commands"); } -int main() { +int main() +{ setup(); std::thread image(CapnSend); - std::thread safety([&]() { robot.robotSafety(&message); }); - std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); }); + std::thread safety([&](){ robot.robotSafety(&message); }); + std::thread sendMqtt([&](){ sendKobukiData(robot.parser.data); }); std::thread connectionChecker(checkKobukiConnection); connectionChecker.detach(); // Laat deze thread onafhankelijk draaien + - while (true) { + while (true) + { std::string message = readMQTT(); - if (!message.empty()) { + if (!message.empty()) + { parseMQTT(message); } } @@ -53,42 +56,52 @@ int main() { std::mutex connectionMutex; -void checkKobukiConnection() { - while (true) { +void checkKobukiConnection() +{ + while (true) { - std::lock_guard lock(connectionMutex); - if (!robot.isConnected()) { - if (kobuki_connected) { - cout << "Kobuki is disconnected" << endl; - kobuki_connected = false; + { + std::lock_guard lock(connectionMutex); + if (!robot.isConnected()) + { + if (kobuki_connected) + { + cout << "Kobuki is disconnected" << endl; + kobuki_connected = false; + } + // Probeer opnieuw te verbinden + cout << "Attempting to reconnect Kobuki..." << endl; + robot.startCommunication("/dev/ttyUSB0", true, nullptr); + kobuki_connected = robot.isConnected(); + if (kobuki_connected) + { + cout << "Kobuki reconnected successfully!" << endl; + } + else + { + cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl; + } + }else{ + if (!kobuki_connected) + { + cout << "Kobuki is connected" << endl; + kobuki_connected = true; + } } - // Probeer opnieuw te verbinden - cout << "Attempting to reconnect Kobuki..." << endl; - robot.startCommunication("/dev/ttyUSB0", true, nullptr); - kobuki_connected = robot.isConnected(); - if (kobuki_connected) { - cout << "Kobuki reconnected successfully!" << endl; - } else { - cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." - << endl; - } - } else { - if (!kobuki_connected) { - cout << "Kobuki is connected" << endl; - kobuki_connected = true; - } - } - std::this_thread::sleep_for( - std::chrono::seconds(5)); // Controleer elke 5 seconden + std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden } - } +} } -std::string readMQTT() { + + +std::string readMQTT() +{ static std::string lastMessage; std::string message = client.getLastMessage(); - if (!message.empty() && message != lastMessage) { + if (!message.empty() && message != lastMessage) + { std::cout << "MQTT Message: " << message << std::endl; lastMessage = message; } @@ -98,32 +111,49 @@ std::string readMQTT() { return lastMessage; } -void parseMQTT(std::string message) { - if (message == "up") { +void parseMQTT(std::string message) +{ + if (message == "up") + { robot.forward(350); - } else if (message == "left") { + } + else if (message == "left") + { robot.setRotationSpeed(4); - } else if (message == "right") { + } + else if (message == "right") + { robot.setRotationSpeed(-4); - } else if (message == "down") { + } + else if (message == "down") + { robot.forward(-350); - } else if (message == "stop") { + } + else if (message == "stop") + { robot.sendNullMessage(); robot.sendNullMessage(); - } else if (message == "estop") { + } + else if (message == "estop") + { robot.forward(-400); - } else { + } + else + { std::cout << "Invalid command" << std::endl; } } -void logToFile() { - while (true) { +void logToFile() +{ + while (true) + { TKobukiData robotData = robot.parser.data; std::ofstream outputFile("log", std::ios_base::app); // Open file in append mode to // not overwrite own content - if (outputFile.is_open()) { // check if the file was opened successfully + if (outputFile.is_open()) + { // check if the file was opened successfully // Get current time std::time_t now = std::time(nullptr); outputFile << "Timestamp: " << std::ctime(&now); @@ -181,7 +211,9 @@ void logToFile() { outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n"; outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n"; outputFile.close(); - } else { + } + else + { std::cerr << "Error opening file\n"; } @@ -189,8 +221,10 @@ void logToFile() { } } -void sendIndividualKobukiData(const TKobukiData &data) { - while (true) { +void sendIndividualKobukiData(const TKobukiData &data) +{ + while (true) + { std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: " << data.timestamp << std::endl; client.publishMessage("kobuki/data/timestamp", @@ -278,7 +312,8 @@ void sendIndividualKobukiData(const TKobukiData &data) { client.publishMessage("kobuki/data/extraInfo/UDID2", std::to_string(data.extraInfo.UDID2)); - if (!data.gyroData.empty()) { + if (!data.gyroData.empty()) + { const auto &latestGyro = data.gyroData.back(); client.publishMessage("kobuki/data/gyroData/x", std::to_string(latestGyro.x)); @@ -292,7 +327,8 @@ void sendIndividualKobukiData(const TKobukiData &data) { } } -std::string serializeKobukiData(const TKobukiData &data) { +std::string serializeKobukiData(const TKobukiData &data) +{ std::string json = "{\"timestamp\":" + std::to_string(data.timestamp) + ",\"BumperCenter\":" + std::to_string(data.BumperCenter) + @@ -345,7 +381,8 @@ std::string serializeKobukiData(const TKobukiData &data) { ",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) + ",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":["; - if (!data.gyroData.empty()) { + if (!data.gyroData.empty()) + { const auto &latestGyro = data.gyroData.back(); json += "{\"x\":" + std::to_string(latestGyro.x) + ",\"y\":" + std::to_string(latestGyro.y) + @@ -357,8 +394,10 @@ std::string serializeKobukiData(const TKobukiData &data) { } // create extra function to send the message every 100ms // needed it so it can be threaded -void sendKobukiData(TKobukiData &data) { - while (true) { +void sendKobukiData(TKobukiData &data) +{ + while (true) + { client.publishMessage("kobuki/data", serializeKobukiData(data)); std::cout << "Sent data" << std::endl; std::this_thread::sleep_for(std::chrono::milliseconds(1000)); @@ -366,41 +405,39 @@ void sendKobukiData(TKobukiData &data) { } void CapnSend() { - VideoCapture cap(0); - if (!cap.isOpened()) { - cerr << "Error: Could not open camera" << endl; - return; - } - - Mat frame; - while (true) { - if (!cap.read(frame)) { - cout << "Reconnecting camera" << endl; - cap.release(); - std::this_thread::sleep_for(std::chrono::seconds(1)); - // Attempt to reconnect to the camera - cap.open(0); - if (!cap.isOpened()) { - cerr << "Error: Could not reconnect to camera" << endl; - std::this_thread::sleep_for( - std::chrono::seconds(1)); // Wait before retrying - continue; - } else { - cout << "Reconnected to camera" << endl; - continue; - } + VideoCapture cap(0); + if (!cap.isOpened()) { + cerr << "Error: Could not open camera" << endl; + return; } - // Convert the image to a byte array - vector buf; - imencode(".jpg", frame, buf); - auto *enc_msg = reinterpret_cast(buf.data()); + Mat frame; + while (true) { + if (!cap.read(frame)) { + cout << "Reconnecting camera" << endl; + cap.release(); + std::this_thread::sleep_for(std::chrono::seconds(1)); + // Attempt to reconnect to the camera + cap.open(0); + if (!cap.isOpened()) { + cerr << "Error: Could not reconnect to camera" << endl; + std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait before retrying + continue; + } else { + cout << "Reconnected to camera" << endl; + continue; + } + } - // Publish the image data - client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size())); - cout << "Sent image" << endl; + // Convert the image to a byte array + vector buf; + imencode(".jpg", frame, buf); + auto *enc_msg = reinterpret_cast(buf.data()); - std::this_thread::sleep_for( - std::chrono::milliseconds(200)); // Send image every 200ms - } + // Publish the image data + client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size())); + cout << "Sent image" << endl; + + std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Send image every 200ms + } } \ No newline at end of file