From eaee4ebd60258484fad931d1a492a61a9819d16d Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Tue, 14 Jan 2025 15:06:54 +0100 Subject: [PATCH] change port --- src/C++/Driver/src/main.cpp | 290 +----------------------------------- 1 file changed, 4 insertions(+), 286 deletions(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 988d214..0c20b11 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -1,292 +1,10 @@ +#include #include -#include +#include #include -#include "MQTT/MqttClient.h" -#include "KobukiDriver/CKobuki.h" -#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 -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)); - } -} +using namespace std; void CapnSend() { int fps = 15; @@ -299,7 +17,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 ! mpegtsmux ! udpsink host=127.0.0.1 port=5000", + 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", CAP_GSTREAMER, 0, fps, Size(width, height), true); if (!out.isOpened()) { cerr << "Error: Can't open video writer" << endl;