increase image compression level and re-enabled thread

opencv camera logic rewrite

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

docs/code/kobuki-reconnect.md

@@ -0,0 +1,78 @@
+# Kobuki automatische reconnect
+
+Mijn taak was om de kobuki automatisch te reconnecten als de verbinding verbroken werd met de pi. nu moet je telkens handmatig de pi laten connecten met de kobuki, dit is niet handig als iemand niet weet hoe dit moet. 
+
+De connectie word gemaakt met ttyUSB0. Dat is de eerste poort die wij kunnen gebruiken voor de kobuki. 
+
+```cpp
+  robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
+```
+
+ik heb een functie gemaakt die kijkt of de pi nog is verbonden aan de kobuki. in de if statement kijkt hij elke 5 seconden of de ttyusb0 poort beschikbaar is. 
+
+```cpp
+void checkKobukiConnection()
+{
+    while (true)
+    {
+        std::lock_guard<std::mutex> lock(connectionMutex);
+
+        // 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
+        }
+```	
+
+Hier kijk ik dan of de kobuki is geconnect, zoniet dan moet ie weer connecten met ttyUSB0
+
+```cpp
+ // 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("/dev/ttyUSB0", true, nullptr);
+
+            if (robot.isConnected()){
+                cout << "Kobuki reconnected successfully!" << endl;
+                kobuki_connected = true;
+            }
+            else{
+                cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl;
+            }
+        }
+```        
+
+Nu heb ik het probleem dat als ik de kabel eruit steek en terug in de kobuki stop dat de pi niet meer wil connecten. Dit komt omdat het systeem denkt dat de poort "ttyUSB0" nog steeds gebruikt word. Als ik de kabel dan terug stop word de poort "ttyUSB1" gebruikt, omdat ttyusb0 niet word vrijgegeven.
+
+```bash
+ishak@raspberrypi:~ $ dmesg | tail -n 20
+[10516.084132] usb 1-1.3: Product: iClebo Kobuki
+[10516.084144] usb 1-1.3: Manufacturer: Yujin Robot
+[10516.084155] usb 1-1.3: SerialNumber: kobuki_AI02MQMK
+[10516.091210] ftdi_sio 1-1.3:1.0: FTDI USB Serial Device converter detected
+[10516.091414] usb 1-1.3: Detected FT232R
+[10516.099169] usb 1-1.3: FTDI USB Serial Device converter now attached to ttyUSB1
+[10574.100491] usb 1-1.3: USB disconnect, device number 34
+[10574.101596] ftdi_sio ttyUSB1: FTDI USB Serial Device converter now disconnected from ttyUSB1
+[10574.101735] ftdi_sio 1-1.3:1.0: device disconnected
+[10579.697816] usb 1-1.3: new full-speed USB device number 35 using dwc_otg
+[10579.829776] usb 1-1.3: New USB device found, idVendor=0403, idProduct=6001, bcdDevice= 6.00
+[10579.829821] usb 1-1.3: New USB device strings: Mfr=1, Product=2, SerialNumber=3
+[10579.829836] usb 1-1.3: Product: iClebo Kobuki
+[10579.829848] usb 1-1.3: Manufacturer: Yujin Robot
+[10579.829860] usb 1-1.3: SerialNumber: kobuki_AI02MQMK
+[10579.840148] ftdi_sio 1-1.3:1.0: FTDI USB Serial Device converter detected
+[10579.840351] usb 1-1.3: Detected FT232R
+[10579.842208] usb 1-1.3: FTDI USB Serial Device converter now attached to ttyUSB1
+[10612.745819] hwmon hwmon1: Voltage normalised
+[10614.761829] hwmon hwmon1: Undervoltage detected!
+```

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,40 +15,34 @@ 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);
 
-void setup()
+std::string findKobukiPort()
 {
-  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");
+    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 checkKobukiConnection()
+void setup()
 {
-  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
-  }
+  std::string port = findKobukiPort();
+  unsigned char *null_ptr(0);
+  robot.startCommunication(const_cast<char*>(port.c_str()), true, null_ptr);
+  // connect mqtt server and sub to commands
+  client.connect();
+  client.subscribe("home/commands");
 }
 
 int main()
@@ -55,6 +51,9 @@ int main()
   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 +69,47 @@ int main()
   image.join();
 }
 
+std::mutex connectionMutex;
+
+void checkKobukiConnection()
+{
+    while (true)
+    {
+        std::lock_guard<std::mutex> lock(connectionMutex);
+        std::string port = findKobukiPort();
+
+        if (port.empty()) {
+            if (kobuki_connected) {
+                cout << "Kobuki disconnected: No USB device 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;
@@ -367,6 +407,7 @@ std::string serializeKobukiData(const TKobukiData &data)
   json += "]}";
   return json;
 }
+
 // create extra function to send the message every 100ms
 // needed it so it can be threaded
 void sendKobukiData(TKobukiData &data)
@@ -380,7 +421,8 @@ void sendKobukiData(TKobukiData &data)
 }
 
 void CapnSend() {
-    VideoCapture cap(0);
+  int COMPRESSION_LEVEL = 90;
+    VideoCapture cap(0); // Open the camera
     if (!cap.isOpened()) {
         cerr << "Error: Could not open camera" << endl;
         return;
@@ -404,14 +446,21 @@ void CapnSend() {
             }
         }
 
-        // Convert the image to a byte array
-        vector<uchar> buf;
-        imencode(".jpg", frame, buf);
-        auto *enc_msg = reinterpret_cast<unsigned char *>(buf.data());
 
-        // Publish the image data
-        client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
-        cout << "Sent image" << endl;
+        vector<uchar> imgbuf;
+        vector<int> compression_params;
+        compression_params.push_back(IMWRITE_JPEG_QUALITY); // Set JPEG quality
+        compression_params.push_back(COMPRESSION_LEVEL);  // Adjust the quality level (0-100, lower = more compression)
+
+        // Encode the image into the byte buffer with the specified compression parameters
+        imencode(".jpg", frame, imgbuf, compression_params);
+
+        // Convert the vector<uchar> buffer to a string (no casting)
+        string enc_msg(imgbuf.begin(), imgbuf.end());
+
+        // Publish the compressed image data (MQTT, in this case)
+        client.publishMessage("kobuki/cam", enc_msg);
+        cout << "Sent compressed image" << endl;
 
         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()
@@ -86,6 +85,7 @@ def index():
 @app.route('/control', methods=["GET", "POST"])
 def control():
     if request.authorization and request.authorization.username == 'ishak' and request.authorization.password == 'kobuki':
+        yolo_results_db()
         return render_template('control.html')
     else:
         return ('Unauthorized', 401, {'WWW-Authenticate': 'Basic realm="Login Required"'})
@@ -161,30 +161,25 @@ def image():
 
 @app.route('/yolo_results', methods=['GET'])
 def yolo_results_endpoint():
-    global yolo_results
+    global yolo_results        
+    return jsonify(yolo_results)
+
+def yolo_results_db():
+    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)"
-            yolo_tuples = [(result["class"], result["confidence"]) for result in yolo_results]
-            print(f"YOLO Tuples: {yolo_tuples}")  # Debug statement
-            cursor.executemany(sql_yolo, yolo_tuples)
-            db.commit()
-            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()
-
+        try:
+            db = get_db()
+            with db.cursor() as cursor:
+                sql_yolo = "INSERT INTO image (class, confidence) VALUES (%s, %s)"
+                yolo_tuples = [(result["class"], result["confidence"]) for result in yolo_results]
+                print(f"YOLO Tuples: {yolo_tuples}")  # Debug statement
+                cursor.executemany(sql_yolo, yolo_tuples)
+                db.commit()
+                cursor.close()
+        except mysql.connector.Error as err:
+            print(f"Database error: {err}")
+        except Exception as e:
+            print(f"Unexpected error: {e}")
 
 if __name__ == '__main__':
     app.run(debug=True, port=5000)