From 7f786d5197b4d52581afa50d8238e2a89a3889cc Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Wed, 11 Dec 2024 15:37:31 +0100 Subject: [PATCH] change camera logic --- src/C++/Driver/src/main.cpp | 57 +++++++++++++++++++++---------------- 1 file changed, 32 insertions(+), 25 deletions(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 10b0b94..1975979 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -293,32 +293,39 @@ void sendKobukiData(TKobukiData &data) void CapnSend() { - std::string pipeline = "libcamerasrc ! video/x-raw,width=640,height=480,framerate=30/1 ! videoconvert ! appsink"; - VideoCapture cap(pipeline, cv::CAP_GSTREAMER); - if (!cap.isOpened()) + VideoCapture cap; + if (!cap.open("/dev/video0", cv::CAP_V4L2)) { - 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 open camera with /dev/video0" << endl; + if (!cap.open("/dev/video1", cv::CAP_V4L2)) { - cerr << "Error: Could not capture image" << endl; - continue; + cerr << "Error: Could not open camera with /dev/video1" << endl; + if (!cap.open("/dev/media2", cv::CAP_V4L2)) + { + cerr << "Error: Could not open camera with /dev/video1" << endl; + return; + } + } + } + + Mat frame; + while (true) + { + cap >> frame; // Capture a new image frame + if (frame.empty()) + { + cerr << "Error: Could not capture image" << endl; + continue; + } + + // Convert the image to a byte array + vector buf; + imencode(".jpg", frame, buf); + auto *enc_msg = reinterpret_cast(buf.data()); + + // Publish the image data + client.publishMessage("kobuki/cam-", string(enc_msg, enc_msg + buf.size())); + cout << "Sent image" << endl; + std::this_thread::sleep_for(std::chrono::milliseconds(400)); // Send image every 1000ms } - - // Convert the image to a byte array - vector buf; - imencode(".jpg", frame, buf); - auto *enc_msg = reinterpret_cast(buf.data()); - - // Publish the image data - client.publishMessage("kobuki/cam-", string(enc_msg, enc_msg + buf.size())); - cout << "Sent image" << endl; - std::this_thread::sleep_for(std::chrono::milliseconds(400)); // Send image every 1000ms } -}