camera reconnection added

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

@@ -10,11 +10,10 @@ using namespace cv;
 CKobuki robot;
 std::atomic<bool> kobuki_connected(false);
 
-
 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);
@@ -24,19 +23,24 @@ 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");
 }
 
-void checkKobukiConnection() {
+void checkKobukiConnection()
-    while (true) {
+{
+  while (true)
+  {
     bool connected = robot.isConnected();
-        if (!connected && kobuki_connected) {
+    if (!connected && kobuki_connected)
+    {
       cout << "Kobuki is disconnected" << endl;
       kobuki_connected = false;
-        } else if (connected && !kobuki_connected) {
+    }
+    else if (connected && !kobuki_connected)
+    {
       cout << "Kobuki is connecting..." << endl;
       // Start de Kobuki automatisch
       robot.startCommunication("/dev/ttyUSB0", true, nullptr);
@@ -48,17 +52,17 @@ void checkKobukiConnection() {
 int main()
 {
   setup();
-    std::thread image (CapnSend);
+  std::thread image(CapnSend);
-    std::thread safety([&]() { robot.robotSafety(&message); });
+  std::thread safety([&](){ robot.robotSafety(&message); });
-    std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
+  std::thread sendMqtt([&](){ sendKobukiData(robot.parser.data); });
-    std::thread connectionThread(checkKobukiConnection);
 
-    while(true){
+  while (true)
+  {
     std::string message = readMQTT();
-        if (!message.empty()){
+    if (!message.empty())
+    {
       parseMQTT(message);
     }
-        
   }
 
   sendMqtt.join();
@@ -83,32 +87,49 @@ std::string readMQTT()
   return lastMessage;
 }
 
-void parseMQTT(std::string message) {
+void parseMQTT(std::string message)
-  if (message == "up") {
+{
+  if (message == "up")
+  {
     robot.forward(350);
-  } else if (message == "left") {
+  }
+  else if (message == "left")
+  {
     robot.setRotationSpeed(4);
-  } else if (message == "right") {
+  }
+  else if (message == "right")
+  {
     robot.setRotationSpeed(-4);
-  } else if (message == "down") {
+  }
+  else if (message == "down")
+  {
     robot.forward(-350);
-  } else if (message == "stop") {
+  }
+  else if (message == "stop")
+  {
     robot.sendNullMessage();
     robot.sendNullMessage();
-  } else if (message == "estop") {
+  }
+  else if (message == "estop")
+  {
     robot.forward(-400);
-  } else {
+  }
+  else
+  {
     std::cout << "Invalid command" << std::endl;
   }
 }
 
-void logToFile() {
+void logToFile()
-  while (true) {
+{
+  while (true)
+  {
     TKobukiData robotData = robot.parser.data;
     std::ofstream outputFile("log",
                              std::ios_base::app); // Open file in append mode to
                                                   // not overwrite own content
-    if (outputFile.is_open()) { // check if the file was opened successfully
+    if (outputFile.is_open())
+    { // check if the file was opened successfully
       // Get current time
       std::time_t now = std::time(nullptr);
       outputFile << "Timestamp: " << std::ctime(&now);
@@ -166,7 +187,9 @@ void logToFile() {
       outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n";
       outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n";
       outputFile.close();
-    } else {
+    }
+    else
+    {
       std::cerr << "Error opening file\n";
     }
 
@@ -174,8 +197,10 @@ void logToFile() {
   }
 }
 
-void sendIndividualKobukiData(const TKobukiData &data) {
+void sendIndividualKobukiData(const TKobukiData &data)
-  while (true) {
+{
+  while (true)
+  {
     std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: "
               << data.timestamp << std::endl;
     client.publishMessage("kobuki/data/timestamp",
@@ -263,7 +288,8 @@ void sendIndividualKobukiData(const TKobukiData &data) {
     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));
@@ -277,7 +303,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) +
@@ -330,7 +357,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) +
@@ -342,25 +370,31 @@ 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);
-    if (!cap.isOpened()) {
+  if (!cap.isOpened())
-        cerr << "Error: Could not open camera" << endl;
+  {
+    cerr << "Error: Could not capture image" << 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;
     }
@@ -368,12 +402,20 @@ void CapnSend() {
     // Convert the image to a byte array
     vector<uchar> buf;
     imencode(".jpg", frame, buf);
-        auto* enc_msg = reinterpret_cast<unsigned char*>(buf.data());
+    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(200)); // Send image every 200ms
+
+    if (!cap.isOpened())
+    {
+      cerr << "Camera disconnected, attempting to reconnect..." << endl;
+
+      cap.open(0);
+      std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait before retrying
+    }
   }
 }