Merge branch 'main' of ssh://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79

2025-08-03 11:55:00 +00:00 · 2025-01-15 14:10:17 +01:00
parent 3367f1dbd2 e273e175cb
commit 95e2d292c9
1 changed files with 127 additions and 86 deletions
--- a/src/C++/Driver/src/main.cpp
+++ b/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);
@@ -22,93 +21,114 @@ void sendKobukiData(TKobukiData &data);

 void setup()
 {
-    unsigned char *null_ptr(0);
-    robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
-    //connect mqtt server and sub to commands
+  unsigned char *null_ptr(0);
+  robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
+  // connect mqtt server and sub to commands

-    client.connect();
-    client.subscribe("home/commands");
+  client.connect();
+  client.subscribe("home/commands");
 }

-void checkKobukiConnection() {
-    while (true) {
-        bool connected = robot.isConnected();
-        if (!connected && kobuki_connected) {
-            cout << "Kobuki is disconnected" << endl;
-            kobuki_connected = false;
-        } else if (connected && !kobuki_connected) {
-            cout << "Kobuki is connecting..." << endl;
-            // Start de Kobuki automatisch
-            robot.startCommunication("/dev/ttyUSB0", true, nullptr);
-        }
-        std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
+void checkKobukiConnection()
+{
+  while (true)
+  {
+    bool connected = robot.isConnected();
+    if (!connected && kobuki_connected)
+    {
+      cout << "Kobuki is disconnected" << endl;
+      kobuki_connected = false;
    }
+    else if (connected && !kobuki_connected)
+    {
+      cout << "Kobuki is connecting..." << endl;
+      // Start de Kobuki automatisch
+      robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+    }
+    std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
+  }
 }

 int main()
 {
-    setup();
-    std::thread image (CapnSend);
-    std::thread safety([&]() { robot.robotSafety(&message); });
-    std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
-    std::thread connectionThread(checkKobukiConnection);
-    
-    while(true){
-        std::string message = readMQTT();
-        if (!message.empty()){
-            parseMQTT(message);
-        }
+  setup();
+  std::thread image(CapnSend);
+  std::thread safety([&](){ robot.robotSafety(&message); });
+  std::thread sendMqtt([&](){ sendKobukiData(robot.parser.data); });

+  while (true)
+  {
+    std::string message = readMQTT();
+    if (!message.empty())
+    {
+      parseMQTT(message);
    }
+  }

-    sendMqtt.join();
-    safety.join();
-    image.join();
-    connectionThread.join();
+  sendMqtt.join();
+  safety.join();
+  image.join();
 }

 std::string readMQTT()
 {
-    static std::string lastMessage;
+  static std::string lastMessage;

-    std::string message = client.getLastMessage();
-    if (!message.empty() && message != lastMessage)
-    {
-        std::cout << "MQTT Message: " << message << std::endl;
-        lastMessage = message;
-    }
+  std::string message = client.getLastMessage();
+  if (!message.empty() && message != lastMessage)
+  {
+    std::cout << "MQTT Message: " << message << std::endl;
+    lastMessage = message;
+  }

  // Add a small delay to avoid busy-waiting
  std::this_thread::sleep_for(std::chrono::milliseconds(100));
  return lastMessage;
 }

-void parseMQTT(std::string message) {
-  if (message == "up") {
+void parseMQTT(std::string message)
+{
+  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() {
-  while (true) {
+void logToFile()
+{
+  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 +186,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 +196,10 @@ void logToFile() {
  }
 }

-void sendIndividualKobukiData(const TKobukiData &data) {
-  while (true) {
+void sendIndividualKobukiData(const TKobukiData &data)
+{
+  while (true)
+  {
    std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: "
              << data.timestamp << std::endl;
    client.publishMessage("kobuki/data/timestamp",
@@ -263,7 +287,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 +302,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 +356,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,38 +369,52 @@ 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) {
-    while (true) {
-        client.publishMessage("kobuki/data", serializeKobukiData(data));
-        std::cout << "Sent data" << std::endl;
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-    }
+void sendKobukiData(TKobukiData &data)
+{
+  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() {
-    VideoCapture cap(0);
-    if (!cap.isOpened()) {
-        cerr << "Error: Could not open camera" << endl;
-        return;
+void CapnSend()
+{
+  VideoCapture cap(0);
+  if (!cap.isOpened())
+  {
+    cerr << "Error: Could not capture image" << endl;
+    return;
+  }
+
+  Mat frame;
+  while (true)
+  {
+    cap >> frame; // Capture a new image frame
+    if (frame.empty())
+    {
+      cerr << "Error: Could not capture image" << endl;
+      continue;
    }

-    Mat frame;
-    while (true) {
-        cap >> frame; // Capture a new image frame
-        if (frame.empty()) {
-            cerr << "Error: Could not capture image" << endl;
-            continue;
-        }
+    // Convert the image to a byte array
+    vector<uchar> buf;
+    imencode(".jpg", frame, buf);
+    auto *enc_msg = reinterpret_cast<unsigned char *>(buf.data());

-        // Convert the image to a byte array
-        vector<uchar> buf;
-        imencode(".jpg", frame, buf);
-        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;

-        // 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

-        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
    }
+  }
 }