From 3d9a68ff7f75977a82249a1868c085ff6e820668 Mon Sep 17 00:00:00 2001 From: "ishak jmilou.ishak" Date: Mon, 20 Jan 2025 11:09:12 +0100 Subject: [PATCH] added else statement --- src/C++/Driver/src/main.cpp | 219 ++++++++++++++++-------------------- 1 file changed, 94 insertions(+), 125 deletions(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 8420633..f378760 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -1,10 +1,11 @@ -#include -#include -#include -#include "MQTT/MqttClient.h" #include "KobukiDriver/CKobuki.h" -#include +#include "MQTT/MqttClient.h" +#include +#include #include +#include +#include + using namespace std; using namespace cv; @@ -16,13 +17,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 @@ -30,21 +31,17 @@ 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); } } @@ -56,46 +53,42 @@ 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; - } - // 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; - } - } + std::lock_guard lock(connectionMutex); + if (!robot.isConnected()) { + if (kobuki_connected) { + cout << "Kobuki is disconnected" << endl; + kobuki_connected = false; } - std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden + // 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::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; } @@ -105,49 +98,32 @@ 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); @@ -205,9 +181,7 @@ void logToFile() outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n"; outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n"; outputFile.close(); - } - else - { + } else { std::cerr << "Error opening file\n"; } @@ -215,10 +189,8 @@ 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", @@ -306,8 +278,7 @@ 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)); @@ -321,8 +292,7 @@ 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) + @@ -375,8 +345,7 @@ 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) + @@ -388,10 +357,8 @@ 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)); @@ -399,39 +366,41 @@ void sendKobukiData(TKobukiData &data) } void CapnSend() { - VideoCapture cap(0); - if (!cap.isOpened()) { - cerr << "Error: Could not open camera" << endl; - return; + 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; + } } - 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; - } - } + // Convert the image to a byte array + vector buf; + imencode(".jpg", frame, buf); + auto *enc_msg = reinterpret_cast(buf.data()); - // Convert the image to a byte array - vector buf; - imencode(".jpg", frame, buf); - auto *enc_msg = reinterpret_cast(buf.data()); + // Publish the image data + client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size())); + cout << "Sent image" << endl; - // 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 - } + std::this_thread::sleep_for( + std::chrono::milliseconds(200)); // Send image every 200ms + } } \ No newline at end of file