opencv camera logic rewrite

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

@@ -379,25 +379,29 @@ void sendKobukiData(TKobukiData &data)
   }
 }
 
-void CapnSend()
-{
+void CapnSend() {
     VideoCapture cap(0);
-  if (!cap.isOpened())
-  {
-    cerr << "Error: Could not capture image" << endl;
+    if (!cap.isOpened()) {
+        cerr << "Error: Could not open camera" << endl;
         return;
     }
 
     Mat frame;
-  while (true)
-  {
-    cap >> frame; // Capture a new image frame
-    if (frame.empty())
-    {
-      cerr << "Error: Could not capture image" << endl;
+    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
@@ -410,13 +414,5 @@ void CapnSend()
         cout << "Sent image" << endl;
 
         std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Send image every 200ms
-
-    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
-    }
     }
 }