diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 1b8997e..494085f 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -387,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 buf; - imencode(".jpg", frame, buf); - auto *enc_msg = reinterpret_cast(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 buf; + imencode(".jpg", frame, buf); + auto *enc_msg = reinterpret_cast(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 } - } } \ No newline at end of file