Merge branch 'main' of ssh://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79

opencv camera logic rewrite

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

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

@@ -1,5 +1,6 @@
 #include <iostream>
 #include <thread>
+#include <fstream>
 #include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
 #include <opencv4/opencv2/opencv.hpp>
@@ -13,6 +14,7 @@ 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";
@@ -24,37 +26,19 @@ 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 +54,30 @@ int main()
   image.join();
 }
 
+std::mutex connectionMutex;
+
+void checkKobukiConnection()
+{
+    while (true)
+    {
+        std::lock_guard<std::mutex> lock(connectionMutex);
+        bool connected = robot.isConnected();
+        if (!connected && kobuki_connected)
+        {
+            cout << "Kobuki is disconnected" << endl;
+            kobuki_connected = false;
+        }
+        else if (connected && !kobuki_connected)
+        {
+            cout << "Kobuki is reconnecting..." << endl;
+            robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+            kobuki_connected = true;
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
+    }
+}
+
+
 std::string readMQTT()
 {
   static std::string lastMessage;
@@ -379,44 +387,40 @@ void sendKobukiData(TKobukiData &data)
   }
 }
 
-void CapnSend()
-{
-  VideoCapture cap(0);
-  if (!cap.isOpened())
-  {
-    cerr << "Error: Could not capture image" << endl;
-    return;
-  }
-
-  Mat frame;
-  while (true)
-  {
-    cap >> frame; // Capture a new image frame
-    if (frame.empty())
-    {
-      cerr << "Error: Could not capture image" << endl;
-      std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait before retrying
-
-      continue;
+void CapnSend() {
+    VideoCapture cap(0);
+    if (!cap.isOpened()) {
+        cerr << "Error: Could not open camera" << endl;
+        return;
     }
 
-    // Convert the image to a byte array
-    vector<uchar> buf;
-    imencode(".jpg", frame, buf);
-    auto *enc_msg = reinterpret_cast<unsigned char *>(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<uchar> buf;
+        imencode(".jpg", frame, buf);
+        auto *enc_msg = reinterpret_cast<unsigned char *>(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;
 
-    if (!cap.isOpened())
-    {
-      cerr << "Camera disconnected, attempting to reconnect..." << endl;
-
-      cap.open(0);
-      std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait before retrying
+        std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Send image every 200ms
     }
-  }
 }

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)