From e682969ec8f5a35a11708bb96687ac1b4f16b5a2 Mon Sep 17 00:00:00 2001
From: Sam Hos <sam.hos@hva.nl>
Date: Wed, 11 Dec 2024 16:07:26 +0100
Subject: [PATCH] code revert

---
 src/C++/Driver/src/main.cpp | 63 +++++++++++++++----------------------
 1 file changed, 26 insertions(+), 37 deletions(-)

diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index ed9c0eb..eec5512 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -12,7 +12,7 @@ CKobuki robot;
 std::string readMQTT();
 void parseMQTT(std::string message);
 void CapnSend();
-// ip, clientID, username, password
+//ip, clientID, username, password
 MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object
 std::string message = "stop";
 std::string serializeKobukiData(const TKobukiData &data);
@@ -22,7 +22,7 @@ void setup()
 {
     unsigned char *null_ptr(0);
     robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
-    // connect mqtt server and sub to commands
+    //connect mqtt server and sub to commands
 
     client.connect();
     client.subscribe("home/commands");
@@ -31,14 +31,11 @@ void setup()
 int main()
 {
     setup();
-    std::thread image(CapnSend);
-    std::thread safety([&]()
-                       { robot.robotSafety(&message); });
-    std::thread sendMqtt([&]()
-                         { sendKobukiData(robot.parser.data); });
-
-    while (true)
-    {
+    std::thread image (CapnSend);
+    std::thread safety([&]() { robot.robotSafety(&message); });
+    std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
+    
+    while(true){
         parseMQTT(readMQTT());
     }
 
@@ -160,10 +157,8 @@ void logToFile()
     }
 }
 
-void sendIndividualKobukiData(const TKobukiData &data)
-{
-    while (true)
-    {
+void sendIndividualKobukiData(const TKobukiData &data) {
+    while (true) {
         client.publishMessage("kobuki/data/timestamp", std::to_string(data.timestamp));
         client.publishMessage("kobuki/data/BumperCenter", std::to_string(data.BumperCenter));
         client.publishMessage("kobuki/data/BumperLeft", std::to_string(data.BumperLeft));
@@ -209,9 +204,8 @@ void sendIndividualKobukiData(const TKobukiData &data)
         client.publishMessage("kobuki/data/extraInfo/UDID1", std::to_string(data.extraInfo.UDID1));
         client.publishMessage("kobuki/data/extraInfo/UDID2", std::to_string(data.extraInfo.UDID2));
 
-        if (!data.gyroData.empty())
-        {
-            const auto &latestGyro = data.gyroData.back();
+        if (!data.gyroData.empty()) {
+            const auto& latestGyro = data.gyroData.back();
             client.publishMessage("kobuki/data/gyroData/x", std::to_string(latestGyro.x));
             client.publishMessage("kobuki/data/gyroData/y", std::to_string(latestGyro.y));
             client.publishMessage("kobuki/data/gyroData/z", std::to_string(latestGyro.z));
@@ -221,8 +215,7 @@ void sendIndividualKobukiData(const TKobukiData &data)
     }
 }
 
-std::string serializeKobukiData(const TKobukiData &data)
-{
+std::string serializeKobukiData(const TKobukiData &data) {
     std::string json = "{\"timestamp\":" + std::to_string(data.timestamp) +
                        ",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
                        ",\"BumperLeft\":" + std::to_string(data.BumperLeft) +
@@ -268,9 +261,8 @@ std::string serializeKobukiData(const TKobukiData &data)
                        ",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
                        ",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
 
-    if (!data.gyroData.empty())
-    {
-        const auto &latestGyro = data.gyroData.back();
+    if (!data.gyroData.empty()) {
+        const auto& latestGyro = data.gyroData.back();
         json += "{\"x\":" + std::to_string(latestGyro.x) +
                 ",\"y\":" + std::to_string(latestGyro.y) +
                 ",\"z\":" + std::to_string(latestGyro.z) + "}";
@@ -279,23 +271,21 @@ std::string serializeKobukiData(const TKobukiData &data)
     json += "]}";
     return json;
 }
-// create extra function to send the message every 100ms
-// needed it so it can be threaded
-void sendKobukiData(TKobukiData &data)
-{
-    while (true)
-    {
+//create extra function to send the message every 100ms
+//needed it so it can be threaded
+void sendKobukiData(TKobukiData &data) {
+    while (true) {
         client.publishMessage("kobuki/data", serializeKobukiData(data));
         std::cout << "Sent data" << std::endl;
         std::this_thread::sleep_for(std::chrono::milliseconds(1000));
     }
 }
+
+
 void CapnSend() {
-    // Use GStreamer pipeline to access the PiCam
-    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()) {
-        cerr << "Error: Could not open camera with GStreamer pipeline" << endl;
+    VideoCapture cap(0);
+     if (!cap.isOpened()) {
+        cerr << "Error: Could not open camera" << endl;
         return;
     }
 
@@ -313,9 +303,8 @@ void CapnSend() {
         auto* enc_msg = reinterpret_cast<unsigned char*>(buf.data());
 
         // Publish the image data
-        client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
+        client.publishMessage("kobuki/cam-", string(enc_msg, enc_msg + buf.size()));
         cout << "Sent image" << endl;
-
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Send image every 1000ms
+        std::this_thread::sleep_for(std::chrono::milliseconds(400)); // Send image every 1000ms
     }
-}
\ No newline at end of file
+}