From e04cff3d6546c1163a060c167ea9e0c2887ccb39 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Thu, 16 Jan 2025 12:28:40 +0100 Subject: [PATCH] opencv camera logic rewrite --- src/C++/Driver/src/main.cpp | 64 +++++++++++++++++-------------------- 1 file changed, 30 insertions(+), 34 deletions(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 5fb1c69..6bb6fee 100644 --- a/src/C++/Driver/src/main.cpp +++ b/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 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