diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 7473fae..9101493 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -39,6 +39,15 @@ int main() { std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); }); while (true) { + if (!robot.isConnected()) { + std::cout << "Kobuki is not connected anymore. Reconnecting..." << std::endl; + robot.startCommunication("/dev/ttyUSB0", true, nullptr); + while (!robot.isConnected()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + std::cout << "Reconnected to Kobuki." << std::endl; + } + std::string message = readMQTT(); if (!message.empty()) { parseMQTT(message);