add pipeline for picam

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

@@ -32,10 +32,13 @@ int main()
 {
     setup();
     std::thread image(CapnSend);
-    std::thread safety([&]() { robot.robotSafety(&message); });
+    std::thread safety([&]()
-    std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
+                       { robot.robotSafety(&message); });
+    std::thread sendMqtt([&]()
+                         { sendKobukiData(robot.parser.data); });
 
-    while(true){
+    while (true)
+    {
         parseMQTT(readMQTT());
     }
 
@@ -157,8 +160,10 @@ void logToFile()
     }
 }
 
-void sendIndividualKobukiData(const TKobukiData &data) {
+void sendIndividualKobukiData(const TKobukiData &data)
-    while (true) {
+{
+    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));
@@ -204,7 +209,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()) {
+        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));
@@ -215,7 +221,8 @@ 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) +
@@ -261,7 +268,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()) {
+    if (!data.gyroData.empty())
+    {
         const auto &latestGyro = data.gyroData.back();
         json += "{\"x\":" + std::to_string(latestGyro.x) +
                 ",\"y\":" + std::to_string(latestGyro.y) +
@@ -273,26 +281,32 @@ std::string serializeKobukiData(const TKobukiData &data) {
 }
 // create extra function to send the message every 100ms
 // needed it so it can be threaded
-void sendKobukiData(TKobukiData &data) {
+void sendKobukiData(TKobukiData &data)
-    while (true) {
+{
+    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()
-void CapnSend() {
+{
-    VideoCapture cap(0, cv::CAP_V4L2); // Open the default camera and set it to the V4L2 backend
+    std::string pipeline = "libcamerasrc ! video/x-raw,width=640,height=480,framerate=30/1 ! videoconvert ! appsink";
-     if (!cap.isOpened()) {
+    VideoCapture cap(pipeline, cv::CAP_GSTREAMER);
+    if (!cap.isOpened())
+    {
         cerr << "Error: Could not open camera" << endl;
         return;
     }
 
     Mat frame;
-    while (true) {
+    while (true)
+    {
         cap >> frame; // Capture a new image frame
-        if (frame.empty()) {
+        if (frame.empty())
+        {
             cerr << "Error: Could not capture image" << endl;
             continue;
         }