From 7d3a5fa9a32c62be87b3c71d853c0f1487781195 Mon Sep 17 00:00:00 2001
From: "ishak jmilou.ishak" <ishak.jmiloui@hva.nl>
Date: Thu, 9 Jan 2025 15:43:45 +0100
Subject: [PATCH] fix: improve Kobuki communication startup and reconnection
 logging

---
 src/C++/Driver/src/main.cpp | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index 9337292..6d2c67e 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -1,13 +1,10 @@
 #include <iostream>
-#include <cmath>
 #include <thread>
 #include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
 #include <opencv4/opencv2/opencv.hpp>
 #include <opencv4/opencv2/core.hpp>
 
-#include <thread>
-
 using namespace std;
 using namespace cv;
 CKobuki robot;
@@ -15,7 +12,6 @@ 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";
 std::string serializeKobukiData(const TKobukiData &data);
@@ -23,12 +19,16 @@ void sendKobukiData(TKobukiData &data);
 
 void setup() {
   unsigned char *null_ptr(0);
-  robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
+  std::cout << "Attempting to start communication with Kobuki..." << std::endl;
+  if (!robot.startCommunication("/dev/ttyUSB0", true, null_ptr)) {
+    std::cerr << "Failed to start communication with Kobuki." << std::endl;
+  } else {
+    std::cout << "Successfully started communication with Kobuki." << std::endl;
+  }
   // connect mqtt server and sub to commands
 
   client.connect();
   client.subscribe("home/commands");
-
 }
 
 int main() {
@@ -41,7 +41,9 @@ int main() {
   while (true) {
     if (!robot.isConnected()) {
       std::cout << "Kobuki is not connected anymore. Reconnecting..." << std::endl;
-      robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+      if (!robot.startCommunication("/dev/ttyUSB0", true, nullptr)) {
+        std::cerr << "Failed to reconnect to Kobuki." << std::endl;
+      }
       while (!robot.isConnected()) {
         std::cout << "Attempting to reconnect..." << std::endl;
         std::this_thread::sleep_for(std::chrono::seconds(1));
@@ -71,7 +73,7 @@ std::string readMQTT() {
 
   // Add a small delay to avoid busy-waiting
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  return message;
+  return lastMessage;
 }
 
 void parseMQTT(std::string message) {