From 45247b5574b743c26c2ecbf57f43f1f80d1eabdd Mon Sep 17 00:00:00 2001
From: "ishak jmilou.ishak" <ishak.jmiloui@hva.nl>
Date: Thu, 19 Dec 2024 20:22:43 +0100
Subject: [PATCH] added code from main branch

---
 src/C++/Driver/src/main.cpp | 57 +++++++++++++++++++++++++++++++------
 1 file changed, 48 insertions(+), 9 deletions(-)

diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index af68be6..3d7e147 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -3,11 +3,15 @@
 #include <thread>
 #include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
+#include <opencv4/opencv2/opencv.hpp>
+#include <opencv4/opencv2/core.hpp>
 
 using namespace std;
+using namespace cv;
 CKobuki robot;
 std::string readMQTT();
 void parseMQTT(std::string message);
+void CapnSend();
 //ip, clientID, username, password
 MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object
 std::string message = "stop";
@@ -17,7 +21,7 @@ void sendKobukiData(TKobukiData &data);
 void setup()
 {
     unsigned char *null_ptr(0);
-    // robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
+    robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
     //connect mqtt server and sub to commands
 
     client.connect();
@@ -26,26 +30,33 @@ void setup()
 
 int main()
 {
-    // Unset the http_proxy environment variable
-
-
     setup();
+    std::thread image (CapnSend);
     std::thread safety([&]() { robot.robotSafety(&message); });
     std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
     
     while(true){
-        parseMQTT(readMQTT());
+        std::string message = readMQTT();
+        if (!message.empty()){
+            parseMQTT(message);
+        }
+        
     }
+
     sendMqtt.join();
     safety.join();
+    image.join();
 }
 
 std::string readMQTT()
 {
-    message = client.getLastMessage();
-    if (!message.empty())
+    static std::string lastMessage;
+
+    std::string message = client.getLastMessage();
+    if (!message.empty() && message != lastMessage)
     {
         std::cout << "MQTT Message: " << message << std::endl;
+        lastMessage = message;
     }
 
     // Add a small delay to avoid busy-waiting
@@ -57,7 +68,7 @@ void parseMQTT(std::string message)
 {
     if (message == "up")
     {
-        robot.forward(1024);
+        robot.forward(350);
     }
     else if (message == "left")
     {
@@ -69,7 +80,7 @@ void parseMQTT(std::string message)
     }
     else if (message == "down")
     {
-        robot.forward(-800);
+        robot.forward(-350);
     }
     else if (message == "stop")
     {
@@ -276,3 +287,31 @@ void sendKobukiData(TKobukiData &data) {
         std::this_thread::sleep_for(std::chrono::milliseconds(1000));
     }
 }
+
+void CapnSend() {
+    VideoCapture cap(0);
+    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;
+            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()));
+        cout << "Sent image" << endl;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(300)); // Send image every 1000ms
+    }
+}
\ No newline at end of file