diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp index 1d82a4e..4636a5a 100755 --- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp @@ -94,6 +94,10 @@ int CKobuki::connect(char *comportT) { } } +bool CKobuki::isConnected() { + return (HCom != -1 && tcflush(HCom, TCOFLUSH) == 0); +} + unsigned char *CKobuki::readKobukiMessage() { unsigned char buffer[1]; ssize_t Pocet; diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.h b/src/C++/Driver/src/KobukiDriver/CKobuki.h index 0567df1..31db87e 100755 --- a/src/C++/Driver/src/KobukiDriver/CKobuki.h +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.h @@ -80,6 +80,7 @@ public: void robotSafety(); //overload void sendNullMessage(); bool safetyActive = false; + bool isConnected(); KobukiParser parser; diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 604a5bc..dd57c19 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -1,318 +1,387 @@ -#include -#include -#include -#include "MQTT/MqttClient.h" #include "KobukiDriver/CKobuki.h" -#include +#include "MQTT/MqttClient.h" +#include +#include #include +#include +#include using namespace std; using namespace cv; 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 +void reconnectKobuki(); +void monitorKobukiConnection(); +// 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); 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 +void setup() { + 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"); } -int main() -{ - 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); - } - - } +int main() { + setup(); + reconnectKobuki(); - sendMqtt.join(); - safety.join(); - image.join(); + std::thread connectionMonitor(monitorKobukiConnection); + 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(); } -std::string readMQTT() -{ - static std::string lastMessage; - - std::string message = client.getLastMessage(); - if (!message.empty() && message != lastMessage) - { - std::cout << "MQTT Message: " << message << std::endl; - lastMessage = message; +void reconnectKobuki() { + while (true) { + if (robot.startCommunication("/dev/ttyUSB0") != -1) { + std::cout << "Kobuki opnieuw verbonden!" << std::endl; + break; // Verlaat de loop als de verbinding succesvol is + } else { + std::cerr << "Kobuki niet verbonden. Probeer opnieuw over 3 seconden..." << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(3)); } - - // Add a small delay to avoid busy-waiting - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - return message; + } } -void parseMQTT(std::string message) -{ - if (message == "up") - { - robot.forward(350); - } - else if (message == "left") - { - robot.setRotationSpeed(4); - } - else if (message == "right") - { - robot.setRotationSpeed(-4); - } - else if (message == "down") - { - robot.forward(-350); - } - else if (message == "stop") - { - robot.sendNullMessage(); - robot.sendNullMessage(); - } - else if (message == "estop") - { - robot.forward(-400); - } - else - { - std::cout << "Invalid command" << std::endl; +void monitorKobukiConnection() { + while (true) { + // Check regelmatig of de verbinding actief is + if (!robot.isConnected()) { + std::cerr << "Kobuki verbinding verloren. Reconnectie starten..." << std::endl; + reconnectKobuki(); } + std::this_thread::sleep_for(std::chrono::seconds(5)); // Check iedere 5 seconden + } } -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 - // Get current time - std::time_t now = std::time(nullptr); - outputFile << "Timestamp: " << std::ctime(&now); - // Write data to the file - outputFile << "analogInputCh0: " << robotData.analogInputCh0 << "\n"; - outputFile << "analogInputCh1: " << robotData.analogInputCh1 << "\n"; - outputFile << "analogInputCh2: " << robotData.analogInputCh2 << "\n"; - outputFile << "analogInputCh3: " << robotData.analogInputCh3 << "\n"; - outputFile << "digitalInput: " << robotData.digitalInput << "\n"; - outputFile << "timestamp: " << robotData.timestamp << "\n"; - outputFile << "BumperCenter: " << robotData.BumperCenter << "\n"; - outputFile << "BumperLeft: " << robotData.BumperLeft << "\n"; - outputFile << "BumperRight: " << robotData.BumperRight << "\n"; - outputFile << "WheelDropLeft: " << robotData.WheelDropLeft << "\n"; - outputFile << "WheelDropRight: " << robotData.WheelDropRight << "\n"; - outputFile << "CliffCenter: " << robotData.CliffCenter << "\n"; - outputFile << "CliffLeft: " << robotData.CliffLeft << "\n"; - outputFile << "CliffRight: " << robotData.CliffRight << "\n"; - outputFile << "EncoderLeft: " << robotData.EncoderLeft << "\n"; - outputFile << "EncoderRight: " << robotData.EncoderRight << "\n"; - outputFile << "PWMleft: " << robotData.PWMleft << "\n"; - outputFile << "PWMright: " << robotData.PWMright << "\n"; - outputFile << "ButtonPress: " << robotData.ButtonPress1 << "\n"; - outputFile << "ButtonPress: " << robotData.ButtonPress2 << "\n"; - outputFile << "ButtonPress: " << robotData.ButtonPress3 << "\n"; - outputFile << "Charger: " << robotData.Charger << "\n"; - outputFile << "Battery: " << robotData.Battery << "\n"; - outputFile << "overCurrent: " << robotData.overCurrent << "\n"; - outputFile << "IRSensorRight: " << robotData.IRSensorRight << "\n"; - outputFile << "IRSensorCenter: " << robotData.IRSensorCenter << "\n"; - outputFile << "IRSensorLeft: " << robotData.IRSensorLeft << "\n"; - outputFile << "GyroAngle: " << robotData.GyroAngle << "\n"; - outputFile << "GyroAngleRate: " << robotData.GyroAngleRate << "\n"; - outputFile << "CliffSensorRight: " << robotData.CliffSensorRight << "\n"; - outputFile << "CliffSensorCenter: " << robotData.CliffSensorCenter << "\n"; - outputFile << "CliffSensorLeft: " << robotData.CliffSensorLeft << "\n"; - outputFile << "wheelCurrentLeft: " << robotData.wheelCurrentLeft << "\n"; - outputFile << "wheelCurrentRight: " << robotData.wheelCurrentRight << "\n"; - outputFile << "frameId: " << robotData.frameId << "\n"; - outputFile << "HardwareVersionPatch: " << robotData.extraInfo.HardwareVersionPatch << "\n"; - outputFile << "HardwareVersionMinor: " << robotData.extraInfo.HardwareVersionMinor << "\n"; - outputFile << "HardwareVersionMajor: " << robotData.extraInfo.HardwareVersionMajor << "\n"; - outputFile << "FirmwareVersionPatch: " << robotData.extraInfo.FirmwareVersionPatch << "\n"; - outputFile << "FirmwareVersionMinor: " << robotData.extraInfo.FirmwareVersionMinor << "\n"; - outputFile << "FirmwareVersionMajor: " << robotData.extraInfo.FirmwareVersionMajor << "\n"; - outputFile << "UDID0: " << robotData.extraInfo.UDID0 << "\n"; - outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n"; - outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n"; - outputFile.close(); - } - else - { - std::cerr << "Error opening file\n"; - } - std::this_thread::sleep_for(std::chrono::seconds(2)); // Sleep for 2 seconds +std::string readMQTT() { + static std::string lastMessage; + + 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 message; +} + +void parseMQTT(std::string message) { + if (message == "up") { + robot.forward(350); + } else if (message == "left") { + robot.setRotationSpeed(4); + } else if (message == "right") { + robot.setRotationSpeed(-4); + } else if (message == "down") { + robot.forward(-350); + } else if (message == "stop") { + robot.sendNullMessage(); + robot.sendNullMessage(); + } else if (message == "estop") { + robot.forward(-400); + } else { + std::cout << "Invalid command" << std::endl; + } +} + +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 + // Get current time + std::time_t now = std::time(nullptr); + outputFile << "Timestamp: " << std::ctime(&now); + // Write data to the file + outputFile << "analogInputCh0: " << robotData.analogInputCh0 << "\n"; + outputFile << "analogInputCh1: " << robotData.analogInputCh1 << "\n"; + outputFile << "analogInputCh2: " << robotData.analogInputCh2 << "\n"; + outputFile << "analogInputCh3: " << robotData.analogInputCh3 << "\n"; + outputFile << "digitalInput: " << robotData.digitalInput << "\n"; + outputFile << "timestamp: " << robotData.timestamp << "\n"; + outputFile << "BumperCenter: " << robotData.BumperCenter << "\n"; + outputFile << "BumperLeft: " << robotData.BumperLeft << "\n"; + outputFile << "BumperRight: " << robotData.BumperRight << "\n"; + outputFile << "WheelDropLeft: " << robotData.WheelDropLeft << "\n"; + outputFile << "WheelDropRight: " << robotData.WheelDropRight << "\n"; + outputFile << "CliffCenter: " << robotData.CliffCenter << "\n"; + outputFile << "CliffLeft: " << robotData.CliffLeft << "\n"; + outputFile << "CliffRight: " << robotData.CliffRight << "\n"; + outputFile << "EncoderLeft: " << robotData.EncoderLeft << "\n"; + outputFile << "EncoderRight: " << robotData.EncoderRight << "\n"; + outputFile << "PWMleft: " << robotData.PWMleft << "\n"; + outputFile << "PWMright: " << robotData.PWMright << "\n"; + outputFile << "ButtonPress: " << robotData.ButtonPress1 << "\n"; + outputFile << "ButtonPress: " << robotData.ButtonPress2 << "\n"; + outputFile << "ButtonPress: " << robotData.ButtonPress3 << "\n"; + outputFile << "Charger: " << robotData.Charger << "\n"; + outputFile << "Battery: " << robotData.Battery << "\n"; + outputFile << "overCurrent: " << robotData.overCurrent << "\n"; + outputFile << "IRSensorRight: " << robotData.IRSensorRight << "\n"; + outputFile << "IRSensorCenter: " << robotData.IRSensorCenter << "\n"; + outputFile << "IRSensorLeft: " << robotData.IRSensorLeft << "\n"; + outputFile << "GyroAngle: " << robotData.GyroAngle << "\n"; + outputFile << "GyroAngleRate: " << robotData.GyroAngleRate << "\n"; + outputFile << "CliffSensorRight: " << robotData.CliffSensorRight << "\n"; + outputFile << "CliffSensorCenter: " << robotData.CliffSensorCenter + << "\n"; + outputFile << "CliffSensorLeft: " << robotData.CliffSensorLeft << "\n"; + outputFile << "wheelCurrentLeft: " << robotData.wheelCurrentLeft << "\n"; + outputFile << "wheelCurrentRight: " << robotData.wheelCurrentRight + << "\n"; + outputFile << "frameId: " << robotData.frameId << "\n"; + outputFile << "HardwareVersionPatch: " + << robotData.extraInfo.HardwareVersionPatch << "\n"; + outputFile << "HardwareVersionMinor: " + << robotData.extraInfo.HardwareVersionMinor << "\n"; + outputFile << "HardwareVersionMajor: " + << robotData.extraInfo.HardwareVersionMajor << "\n"; + outputFile << "FirmwareVersionPatch: " + << robotData.extraInfo.FirmwareVersionPatch << "\n"; + outputFile << "FirmwareVersionMinor: " + << robotData.extraInfo.FirmwareVersionMinor << "\n"; + outputFile << "FirmwareVersionMajor: " + << robotData.extraInfo.FirmwareVersionMajor << "\n"; + outputFile << "UDID0: " << robotData.extraInfo.UDID0 << "\n"; + outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n"; + outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n"; + outputFile.close(); + } else { + std::cerr << "Error opening file\n"; } + + std::this_thread::sleep_for(std::chrono::seconds(2)); // Sleep for 2 seconds + } } 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", std::to_string(data.timestamp)); - client.publishMessage("kobuki/data/BumperCenter", std::to_string(data.BumperCenter)); - client.publishMessage("kobuki/data/BumperLeft", std::to_string(data.BumperLeft)); - client.publishMessage("kobuki/data/BumperRight", std::to_string(data.BumperRight)); - client.publishMessage("kobuki/data/WheelDropLeft", std::to_string(data.WheelDropLeft)); - client.publishMessage("kobuki/data/WheelDropRight", std::to_string(data.WheelDropRight)); - client.publishMessage("kobuki/data/CliffCenter", std::to_string(data.CliffCenter)); - client.publishMessage("kobuki/data/CliffLeft", std::to_string(data.CliffLeft)); - client.publishMessage("kobuki/data/CliffRight", std::to_string(data.CliffRight)); - client.publishMessage("kobuki/data/EncoderLeft", std::to_string(data.EncoderLeft)); - client.publishMessage("kobuki/data/EncoderRight", std::to_string(data.EncoderRight)); - client.publishMessage("kobuki/data/PWMleft", std::to_string(data.PWMleft)); - client.publishMessage("kobuki/data/PWMright", std::to_string(data.PWMright)); - client.publishMessage("kobuki/data/ButtonPress1", std::to_string(data.ButtonPress1)); - client.publishMessage("kobuki/data/ButtonPress2", std::to_string(data.ButtonPress2)); - client.publishMessage("kobuki/data/ButtonPress3", std::to_string(data.ButtonPress3)); - client.publishMessage("kobuki/data/Charger", std::to_string(data.Charger)); - client.publishMessage("kobuki/data/Battery", std::to_string(data.Battery)); - client.publishMessage("kobuki/data/overCurrent", std::to_string(data.overCurrent)); - client.publishMessage("kobuki/data/IRSensorRight", std::to_string(data.IRSensorRight)); - client.publishMessage("kobuki/data/IRSensorCenter", std::to_string(data.IRSensorCenter)); - client.publishMessage("kobuki/data/IRSensorLeft", std::to_string(data.IRSensorLeft)); - client.publishMessage("kobuki/data/GyroAngle", std::to_string(data.GyroAngle)); - client.publishMessage("kobuki/data/GyroAngleRate", std::to_string(data.GyroAngleRate)); - client.publishMessage("kobuki/data/CliffSensorRight", std::to_string(data.CliffSensorRight)); - client.publishMessage("kobuki/data/CliffSensorCenter", std::to_string(data.CliffSensorCenter)); - client.publishMessage("kobuki/data/CliffSensorLeft", std::to_string(data.CliffSensorLeft)); - client.publishMessage("kobuki/data/wheelCurrentLeft", std::to_string(data.wheelCurrentLeft)); - client.publishMessage("kobuki/data/wheelCurrentRight", std::to_string(data.wheelCurrentRight)); - client.publishMessage("kobuki/data/digitalInput", std::to_string(data.digitalInput)); - client.publishMessage("kobuki/data/analogInputCh0", std::to_string(data.analogInputCh0)); - client.publishMessage("kobuki/data/analogInputCh1", std::to_string(data.analogInputCh1)); - client.publishMessage("kobuki/data/analogInputCh2", std::to_string(data.analogInputCh2)); - client.publishMessage("kobuki/data/analogInputCh3", std::to_string(data.analogInputCh3)); - client.publishMessage("kobuki/data/frameId", std::to_string(data.frameId)); - client.publishMessage("kobuki/data/extraInfo/HardwareVersionPatch", std::to_string(data.extraInfo.HardwareVersionPatch)); - client.publishMessage("kobuki/data/extraInfo/HardwareVersionMinor", std::to_string(data.extraInfo.HardwareVersionMinor)); - client.publishMessage("kobuki/data/extraInfo/HardwareVersionMajor", std::to_string(data.extraInfo.HardwareVersionMajor)); - client.publishMessage("kobuki/data/extraInfo/FirmwareVersionPatch", std::to_string(data.extraInfo.FirmwareVersionPatch)); - client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMinor", std::to_string(data.extraInfo.FirmwareVersionMinor)); - client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMajor", std::to_string(data.extraInfo.FirmwareVersionMajor)); - client.publishMessage("kobuki/data/extraInfo/UDID0", std::to_string(data.extraInfo.UDID0)); - client.publishMessage("kobuki/data/extraInfo/UDID1", std::to_string(data.extraInfo.UDID1)); - client.publishMessage("kobuki/data/extraInfo/UDID2", std::to_string(data.extraInfo.UDID2)); + while (true) { + std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: " + << data.timestamp << std::endl; + client.publishMessage("kobuki/data/timestamp", + std::to_string(data.timestamp)); + client.publishMessage("kobuki/data/BumperCenter", + std::to_string(data.BumperCenter)); + client.publishMessage("kobuki/data/BumperLeft", + std::to_string(data.BumperLeft)); + client.publishMessage("kobuki/data/BumperRight", + std::to_string(data.BumperRight)); + client.publishMessage("kobuki/data/WheelDropLeft", + std::to_string(data.WheelDropLeft)); + client.publishMessage("kobuki/data/WheelDropRight", + std::to_string(data.WheelDropRight)); + client.publishMessage("kobuki/data/CliffCenter", + std::to_string(data.CliffCenter)); + client.publishMessage("kobuki/data/CliffLeft", + std::to_string(data.CliffLeft)); + client.publishMessage("kobuki/data/CliffRight", + std::to_string(data.CliffRight)); + client.publishMessage("kobuki/data/EncoderLeft", + std::to_string(data.EncoderLeft)); + client.publishMessage("kobuki/data/EncoderRight", + std::to_string(data.EncoderRight)); + client.publishMessage("kobuki/data/PWMleft", std::to_string(data.PWMleft)); + client.publishMessage("kobuki/data/PWMright", + std::to_string(data.PWMright)); + client.publishMessage("kobuki/data/ButtonPress1", + std::to_string(data.ButtonPress1)); + client.publishMessage("kobuki/data/ButtonPress2", + std::to_string(data.ButtonPress2)); + client.publishMessage("kobuki/data/ButtonPress3", + std::to_string(data.ButtonPress3)); + client.publishMessage("kobuki/data/Charger", std::to_string(data.Charger)); + client.publishMessage("kobuki/data/Battery", std::to_string(data.Battery)); + client.publishMessage("kobuki/data/overCurrent", + std::to_string(data.overCurrent)); + client.publishMessage("kobuki/data/IRSensorRight", + std::to_string(data.IRSensorRight)); + client.publishMessage("kobuki/data/IRSensorCenter", + std::to_string(data.IRSensorCenter)); + client.publishMessage("kobuki/data/IRSensorLeft", + std::to_string(data.IRSensorLeft)); + client.publishMessage("kobuki/data/GyroAngle", + std::to_string(data.GyroAngle)); + client.publishMessage("kobuki/data/GyroAngleRate", + std::to_string(data.GyroAngleRate)); + client.publishMessage("kobuki/data/CliffSensorRight", + std::to_string(data.CliffSensorRight)); + client.publishMessage("kobuki/data/CliffSensorCenter", + std::to_string(data.CliffSensorCenter)); + client.publishMessage("kobuki/data/CliffSensorLeft", + std::to_string(data.CliffSensorLeft)); + client.publishMessage("kobuki/data/wheelCurrentLeft", + std::to_string(data.wheelCurrentLeft)); + client.publishMessage("kobuki/data/wheelCurrentRight", + std::to_string(data.wheelCurrentRight)); + client.publishMessage("kobuki/data/digitalInput", + std::to_string(data.digitalInput)); + client.publishMessage("kobuki/data/analogInputCh0", + std::to_string(data.analogInputCh0)); + client.publishMessage("kobuki/data/analogInputCh1", + std::to_string(data.analogInputCh1)); + client.publishMessage("kobuki/data/analogInputCh2", + std::to_string(data.analogInputCh2)); + client.publishMessage("kobuki/data/analogInputCh3", + std::to_string(data.analogInputCh3)); + client.publishMessage("kobuki/data/frameId", std::to_string(data.frameId)); + client.publishMessage("kobuki/data/extraInfo/HardwareVersionPatch", + std::to_string(data.extraInfo.HardwareVersionPatch)); + client.publishMessage("kobuki/data/extraInfo/HardwareVersionMinor", + std::to_string(data.extraInfo.HardwareVersionMinor)); + client.publishMessage("kobuki/data/extraInfo/HardwareVersionMajor", + std::to_string(data.extraInfo.HardwareVersionMajor)); + client.publishMessage("kobuki/data/extraInfo/FirmwareVersionPatch", + std::to_string(data.extraInfo.FirmwareVersionPatch)); + client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMinor", + std::to_string(data.extraInfo.FirmwareVersionMinor)); + client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMajor", + std::to_string(data.extraInfo.FirmwareVersionMajor)); + client.publishMessage("kobuki/data/extraInfo/UDID0", + std::to_string(data.extraInfo.UDID0)); + client.publishMessage("kobuki/data/extraInfo/UDID1", + std::to_string(data.extraInfo.UDID1)); + client.publishMessage("kobuki/data/extraInfo/UDID2", + std::to_string(data.extraInfo.UDID2)); - if (!data.gyroData.empty()) { - const auto& latestGyro = data.gyroData.back(); - client.publishMessage("kobuki/data/gyroData/x", std::to_string(latestGyro.x)); - client.publishMessage("kobuki/data/gyroData/y", std::to_string(latestGyro.y)); - client.publishMessage("kobuki/data/gyroData/z", std::to_string(latestGyro.z)); - } - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + if (!data.gyroData.empty()) { + const auto &latestGyro = data.gyroData.back(); + client.publishMessage("kobuki/data/gyroData/x", + std::to_string(latestGyro.x)); + client.publishMessage("kobuki/data/gyroData/y", + std::to_string(latestGyro.y)); + client.publishMessage("kobuki/data/gyroData/z", + std::to_string(latestGyro.z)); } + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } } std::string serializeKobukiData(const TKobukiData &data) { - std::string json = "{\"timestamp\":" + std::to_string(data.timestamp) + - ",\"BumperCenter\":" + std::to_string(data.BumperCenter) + - ",\"BumperLeft\":" + std::to_string(data.BumperLeft) + - ",\"BumperRight\":" + std::to_string(data.BumperRight) + - ",\"WheelDropLeft\":" + std::to_string(data.WheelDropLeft) + - ",\"WheelDropRight\":" + std::to_string(data.WheelDropRight) + - ",\"CliffCenter\":" + std::to_string(data.CliffCenter) + - ",\"CliffLeft\":" + std::to_string(data.CliffLeft) + - ",\"CliffRight\":" + std::to_string(data.CliffRight) + - ",\"EncoderLeft\":" + std::to_string(data.EncoderLeft) + - ",\"EncoderRight\":" + std::to_string(data.EncoderRight) + - ",\"PWMleft\":" + std::to_string(data.PWMleft) + - ",\"PWMright\":" + std::to_string(data.PWMright) + - ",\"ButtonPress1\":" + std::to_string(data.ButtonPress1) + - ",\"ButtonPress2\":" + std::to_string(data.ButtonPress2) + - ",\"ButtonPress3\":" + std::to_string(data.ButtonPress3) + - ",\"Charger\":" + std::to_string(data.Charger) + - ",\"Battery\":" + std::to_string(data.Battery) + - ",\"overCurrent\":" + std::to_string(data.overCurrent) + - ",\"IRSensorRight\":" + std::to_string(data.IRSensorRight) + - ",\"IRSensorCenter\":" + std::to_string(data.IRSensorCenter) + - ",\"IRSensorLeft\":" + std::to_string(data.IRSensorLeft) + - ",\"GyroAngle\":" + std::to_string(data.GyroAngle) + - ",\"GyroAngleRate\":" + std::to_string(data.GyroAngleRate) + - ",\"CliffSensorRight\":" + std::to_string(data.CliffSensorRight) + - ",\"CliffSensorCenter\":" + std::to_string(data.CliffSensorCenter) + - ",\"CliffSensorLeft\":" + std::to_string(data.CliffSensorLeft) + - ",\"wheelCurrentLeft\":" + std::to_string(data.wheelCurrentLeft) + - ",\"wheelCurrentRight\":" + std::to_string(data.wheelCurrentRight) + - ",\"digitalInput\":" + std::to_string(data.digitalInput) + - ",\"analogInputCh0\":" + std::to_string(data.analogInputCh0) + - ",\"analogInputCh1\":" + std::to_string(data.analogInputCh1) + - ",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) + - ",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) + - ",\"frameId\":" + std::to_string(data.frameId) + - ",\"extraInfo\":{\"HardwareVersionPatch\":" + std::to_string(data.extraInfo.HardwareVersionPatch) + - ",\"HardwareVersionMinor\":" + std::to_string(data.extraInfo.HardwareVersionMinor) + - ",\"HardwareVersionMajor\":" + std::to_string(data.extraInfo.HardwareVersionMajor) + - ",\"FirmwareVersionPatch\":" + std::to_string(data.extraInfo.FirmwareVersionPatch) + - ",\"FirmwareVersionMinor\":" + std::to_string(data.extraInfo.FirmwareVersionMinor) + - ",\"FirmwareVersionMajor\":" + std::to_string(data.extraInfo.FirmwareVersionMajor) + - ",\"UDID0\":" + std::to_string(data.extraInfo.UDID0) + - ",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) + - ",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":["; + std::string json = + "{\"timestamp\":" + std::to_string(data.timestamp) + + ",\"BumperCenter\":" + std::to_string(data.BumperCenter) + + ",\"BumperLeft\":" + std::to_string(data.BumperLeft) + + ",\"BumperRight\":" + std::to_string(data.BumperRight) + + ",\"WheelDropLeft\":" + std::to_string(data.WheelDropLeft) + + ",\"WheelDropRight\":" + std::to_string(data.WheelDropRight) + + ",\"CliffCenter\":" + std::to_string(data.CliffCenter) + + ",\"CliffLeft\":" + std::to_string(data.CliffLeft) + + ",\"CliffRight\":" + std::to_string(data.CliffRight) + + ",\"EncoderLeft\":" + std::to_string(data.EncoderLeft) + + ",\"EncoderRight\":" + std::to_string(data.EncoderRight) + + ",\"PWMleft\":" + std::to_string(data.PWMleft) + + ",\"PWMright\":" + std::to_string(data.PWMright) + + ",\"ButtonPress1\":" + std::to_string(data.ButtonPress1) + + ",\"ButtonPress2\":" + std::to_string(data.ButtonPress2) + + ",\"ButtonPress3\":" + std::to_string(data.ButtonPress3) + + ",\"Charger\":" + std::to_string(data.Charger) + + ",\"Battery\":" + std::to_string(data.Battery) + + ",\"overCurrent\":" + std::to_string(data.overCurrent) + + ",\"IRSensorRight\":" + std::to_string(data.IRSensorRight) + + ",\"IRSensorCenter\":" + std::to_string(data.IRSensorCenter) + + ",\"IRSensorLeft\":" + std::to_string(data.IRSensorLeft) + + ",\"GyroAngle\":" + std::to_string(data.GyroAngle) + + ",\"GyroAngleRate\":" + std::to_string(data.GyroAngleRate) + + ",\"CliffSensorRight\":" + std::to_string(data.CliffSensorRight) + + ",\"CliffSensorCenter\":" + std::to_string(data.CliffSensorCenter) + + ",\"CliffSensorLeft\":" + std::to_string(data.CliffSensorLeft) + + ",\"wheelCurrentLeft\":" + std::to_string(data.wheelCurrentLeft) + + ",\"wheelCurrentRight\":" + std::to_string(data.wheelCurrentRight) + + ",\"digitalInput\":" + std::to_string(data.digitalInput) + + ",\"analogInputCh0\":" + std::to_string(data.analogInputCh0) + + ",\"analogInputCh1\":" + std::to_string(data.analogInputCh1) + + ",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) + + ",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) + + ",\"frameId\":" + std::to_string(data.frameId) + + ",\"extraInfo\":{\"HardwareVersionPatch\":" + + std::to_string(data.extraInfo.HardwareVersionPatch) + + ",\"HardwareVersionMinor\":" + + std::to_string(data.extraInfo.HardwareVersionMinor) + + ",\"HardwareVersionMajor\":" + + std::to_string(data.extraInfo.HardwareVersionMajor) + + ",\"FirmwareVersionPatch\":" + + std::to_string(data.extraInfo.FirmwareVersionPatch) + + ",\"FirmwareVersionMinor\":" + + std::to_string(data.extraInfo.FirmwareVersionMinor) + + ",\"FirmwareVersionMajor\":" + + std::to_string(data.extraInfo.FirmwareVersionMajor) + + ",\"UDID0\":" + std::to_string(data.extraInfo.UDID0) + + ",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) + + ",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":["; - if (!data.gyroData.empty()) { - const auto& latestGyro = data.gyroData.back(); - json += "{\"x\":" + std::to_string(latestGyro.x) + - ",\"y\":" + std::to_string(latestGyro.y) + - ",\"z\":" + std::to_string(latestGyro.z) + "}"; - } + if (!data.gyroData.empty()) { + const auto &latestGyro = data.gyroData.back(); + json += "{\"x\":" + std::to_string(latestGyro.x) + + ",\"y\":" + std::to_string(latestGyro.y) + + ",\"z\":" + std::to_string(latestGyro.z) + "}"; + } - json += "]}"; - return json; + json += "]}"; + return json; } -//create extra function to send the message every 100ms -//needed it so it can be threaded +// 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)); - } + 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; + VideoCapture cap(0); + if (!cap.isOpened()) { + cerr << "Error: Could not open camera" << 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 buf; + imencode(".jpg", frame, buf); + auto *enc_msg = reinterpret_cast(buf.data()); - // Convert the image to a byte array - vector buf; - imencode(".jpg", frame, buf); - auto* enc_msg = reinterpret_cast(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(300)); // Send image every 1000ms - } + std::this_thread::sleep_for( + std::chrono::milliseconds(300)); // Send image every 1000ms + } } \ No newline at end of file