From ff89b8a742810b5d8eaedf0f0f178cbb6bd8cb80 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Tue, 14 Jan 2025 15:07:44 +0100 Subject: [PATCH] revert and change port number --- src/C++/Driver/src/main.cpp | 290 +++++++++++++++++++++++++++++++++++- 1 file changed, 286 insertions(+), 4 deletions(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 0c20b11..42a8b90 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -1,10 +1,292 @@ -#include #include -#include +#include #include +#include "MQTT/MqttClient.h" +#include "KobukiDriver/CKobuki.h" +#include +#include -using namespace cv; 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 +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 + + 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); + } + + } + + 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; + } + + // 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) { + 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)); + } +} + +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\":["; + + 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; +} +//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 CapnSend() { int fps = 15; @@ -17,7 +299,7 @@ void CapnSend() { return; } - VideoWriter out("appsrc ! videoconvert ! video/x-raw,format=I420 ! x264enc speed-preset=ultrafast bitrate=600 key-int-max=" + to_string(fps * 2) + " ! video/x-h264,profile=baseline ! rtph264pay ! udpsink host=127.0.0.1 port=5001", + VideoWriter out("appsrc ! videoconvert ! video/x-raw,format=I420 ! x264enc speed-preset=ultrafast bitrate=600 key-int-max=" + to_string(fps * 2) + " ! video/x-h264,profile=baseline ! mpegtsmux ! udpsink host=127.0.0.1 port=5001", CAP_GSTREAMER, 0, fps, Size(width, height), true); if (!out.isOpened()) { cerr << "Error: Can't open video writer" << endl;