fix: use const_cast for port string in Kobuki communication

opencv camera logic rewrite

See merge request technische-informatica-sm3/ti-projectten/rooziinuubii79!5

src/C++/Driver/src/main.cpp

@@ -1,5 +1,7 @@
 #include <iostream>
 #include <thread>
+#include <fstream>
+#include <filesystem>
 #include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
 #include <opencv4/opencv2/opencv.hpp>
@@ -13,48 +15,44 @@ std::atomic<bool> kobuki_connected(false);
 std::string readMQTT();
 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
 std::string message = "stop";
 std::string serializeKobukiData(const TKobukiData &data);
 void sendKobukiData(TKobukiData &data);
 
+std::string findKobukiPort()
+{
+    for (const auto& entry : std::filesystem::directory_iterator("/dev"))
+    {
+        std::string device = entry.path().string();
+        if (device.find("ttyUSB") != std::string::npos)
+        {
+            return device; // Returneer de eerste gevonden poort
+        }
+    }
+    return ""; // Geen poort gevonden
+}
+
 void setup()
 {
   unsigned char *null_ptr(0);
   robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
   // connect mqtt server and sub to commands
-
   client.connect();
   client.subscribe("home/commands");
 }
 
-void checkKobukiConnection()
-{
-  while (true)
-  {
-    bool connected = robot.isConnected();
-    if (!connected && kobuki_connected)
-    {
-      cout << "Kobuki is disconnected" << endl;
-      kobuki_connected = false;
-    }
-    else if (connected && !kobuki_connected)
-    {
-      cout << "Kobuki is connecting..." << endl;
-      // Start de Kobuki automatisch
-      robot.startCommunication("/dev/ttyUSB0", true, nullptr);
-    }
-    std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
-  }
-}
-
 int main()
 {
   setup();
   std::thread image(CapnSend);
   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)
   {
@@ -70,6 +68,48 @@ int main()
   image.join();
 }
 
+std::mutex connectionMutex;
+
+void checkKobukiConnection()
+{
+    while (true)
+    {
+        std::lock_guard<std::mutex> lock(connectionMutex);
+        std::string port = findKobukiPort();
+
+        // Controleer of het apparaat beschikbaar is
+        if (!std::ifstream("/dev/ttyUSB0")){
+            if (kobuki_connected){
+                cout << "Kobuki disconnected: USB device not found." << endl;
+                kobuki_connected = false;
+            }
+            std::this_thread::sleep_for(std::chrono::seconds(5));
+            continue; // Probeer later opnieuw
+        }
+
+        // Controleer of de Kobuki verbonden is
+        if (!robot.isConnected()){
+            if (kobuki_connected){
+                cout << "Kobuki disconnected." << endl;
+                kobuki_connected = false;
+            }
+
+            cout << "Attempting to reconnect Kobuki..." << endl;
+            robot.startCommunication(const_cast<char*>(port.c_str()), true, nullptr);
+
+            if (robot.isConnected()){
+                cout << "Kobuki reconnected successfully!" << endl;
+                kobuki_connected = true;
+            }
+            else{
+                cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl;
+            }
+        }
+        // Wacht voordat je opnieuw controleert
+        std::this_thread::sleep_for(std::chrono::seconds(5));
+    }
+}
+
 std::string readMQTT()
 {
   static std::string lastMessage;

src/Python/flask/web/app.py

@@ -48,7 +48,6 @@ def on_message(client, userdata, message):
                     x1, y1, x2, y2 = map(int, box.xyxy[0])
                     cv2.rectangle(processed_image, (x1, y1), (x2, y2), (0, 255, 0), 2)
                     cv2.putText(processed_image, f"{class_name} {box.conf.item():.2f}", (x1, y1 - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2)
-            # yolo_results_db()
 
 # Create an MQTT client instance
 mqtt_client = mqtt.Client()
@@ -163,7 +162,6 @@ def image():
 def yolo_results_endpoint():
     global yolo_results
     with lock:
-        print(f"YOLO Results: {yolo_results}")  # Debug statement
         db = get_db()
         with db.cursor() as cursor:
             sql_yolo = "INSERT INTO image (class, confidence) VALUES (%s, %s)"
@@ -174,17 +172,6 @@ def yolo_results_endpoint():
             cursor.close()
         return jsonify(yolo_results)
 
-# def yolo_results_db():
-#     global yolo_results
-#     with lock:
-#         db = get_db()
-#         with db.cursor() as cursor:
-#             sql_yolo = "INSERT INTO image (object, confidence) VALUES (%s, %s)"
-#             yolo_tuples = [(result["class"], result["confidence"]) for result in yolo_results]
-#             cursor.executemany(sql_yolo, yolo_tuples)
-#             db.commit()
-#             cursor.close()
-
 
 if __name__ == '__main__':
     app.run(debug=True, port=5000)