diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index aa0c5aa..12c310f 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -149,6 +149,64 @@ void logToFile() } } +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) + @@ -212,4 +270,4 @@ void sendKobukiData(TKobukiData &data) { client.publishMessage("kobuki/data", serializeKobukiData(data)); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } -} \ No newline at end of file +}