From 07d88eef7ddad0ec6f0558488f0a6a3a105ddfe9 Mon Sep 17 00:00:00 2001
From: "ishak jmilou.ishak" <ishak.jmiloui@hva.nl>
Date: Thu, 9 Jan 2025 13:19:00 +0100
Subject: [PATCH] fix: improve connection handling and logging in KobukiDriver

---
 src/C++/Driver/src/KobukiDriver/CKobuki.cpp | 20 +++-----------------
 src/C++/Driver/src/main.cpp                 |  7 +++++++
 2 files changed, 10 insertions(+), 17 deletions(-)

diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
index a0a3abe..7b5e0b4 100755
--- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
+++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
@@ -68,28 +68,14 @@ int CKobuki::connect(char *comportT) {
   HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
   if (HCom == -1) {
-    printf("Kobuki connected\n");
+    printf("unable to connect\n");
     return HCom;
   } else {
-    set_interface_attribs2(HCom, B115200,
-                           0); // set speed to 115,200 bps, 8n1 (no parity)
+    set_interface_attribs2(HCom, B115200,0); // set speed to 115,200 bps, 8n1 (no parity)
     set_blocking2(HCom, 0);    // set no blocking
-    /*  struct termios settings;
-      tcgetattr(HCom, &settings);
-
-      cfsetospeed(&settings, B115200); // baud rate
-      settings.c_cflag &= ~PARENB; // no parity
-      settings.c_cflag &= ~CSTOPB; // 1 stop bit
-      settings.c_cflag &= ~CSIZE;
-      settings.c_cflag |= CS8 | CLOCAL; // 8 bits
-      settings.c_lflag &= ~ICANON; // canonical mode
-      settings.c_cc[VTIME]=1;
-      settings.c_oflag &= ~OPOST; // raw output
-
-      tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/
     tcflush(HCom, TCOFLUSH);
 
-    printf("Kobuki connected\n");
+    std::cout<<"Kobuki connected" << std::endl;
     return HCom;
   }
 }
diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index 6a888b7..13b00f1 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -325,6 +325,13 @@ std::string serializeKobukiData(const TKobukiData &data) {
 // needed it so it can be threaded
 void sendKobukiData(TKobukiData &data) {
   while (true) {
+    if(!robot.connected()){
+      std::cout << "Kobuki is not connected anymore" << std::endl;
+      while(!robot.connected()){
+        robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+      }
+    }
     client.publishMessage("kobuki/data", serializeKobukiData(data));
     std::cout << "Sent data" << std::endl;
     std::this_thread::sleep_for(std::chrono::milliseconds(1000));