revert and change port number

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

@@ -289,12 +289,23 @@ void sendKobukiData(TKobukiData &data) {
 }
 
 void CapnSend() {
-    VideoCapture cap(0);
+    int fps = 15;
+    int width = 800;
+    int height = 600;
+
+    VideoCapture cap("/dev/video0");
     if (!cap.isOpened()) {
         cerr << "Error: Could not open camera" << endl;
         return;
     }
 
+    VideoWriter out("appsrc ! videoconvert ! video/x-raw,format=I420 ! x264enc speed-preset=ultrafast bitrate=600 key-int-max=" + to_string(fps * 2) + " ! video/x-h264,profile=baseline ! mpegtsmux ! udpsink host=127.0.0.1 port=5001",
+                    CAP_GSTREAMER, 0, fps, Size(width, height), true);
+    if (!out.isOpened()) {
+        cerr << "Error: Can't open video writer" << endl;
+        return;
+    }
+
     Mat frame;
     while (true) {
         cap >> frame; // Capture a new image frame
@@ -303,15 +314,10 @@ void CapnSend() {
             continue;
         }
 
-        // Convert the image to a byte array
-        vector<uchar> buf;
-        imencode(".jpg", frame, buf);
-        auto* enc_msg = reinterpret_cast<unsigned char*>(buf.data());
-
-        // Publish the image data
-        client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
+        // Write the frame to the UDP stream
+        out.write(frame);
         cout << "Sent image" << endl;
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Send image every 200ms
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / fps)); // Control the frame rate
     }
 }