Merge branch 'usb-reconnect' into 'main'

opencv camera logic rewrite

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

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

@@ -379,44 +379,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
     }
-  }
 }