mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-04 04:14:58 +00:00
made it so it automaticly sends all its data in json to kobuki/data
This commit is contained in:
@@ -12,6 +12,8 @@ void parseMQTT(std::string message);
|
|||||||
MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); // create a client object
|
MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); // create a client object
|
||||||
std::string message = "stop";
|
std::string message = "stop";
|
||||||
std::string serializeKobukiData(const TKobukiData &data);
|
std::string serializeKobukiData(const TKobukiData &data);
|
||||||
|
void sendKobukiData(TKobukiData &data);
|
||||||
|
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
unsigned char *null_ptr(0);
|
unsigned char *null_ptr(0);
|
||||||
@@ -23,13 +25,13 @@ void setup()
|
|||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
setup();
|
setup();
|
||||||
// std::thread safety([&]() { robot.robotSafety(&message); });
|
std::thread safety([&]() { robot.robotSafety(&message); });
|
||||||
// while(true){
|
std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
|
||||||
// parseMQTT(readMQTT());
|
while(true){
|
||||||
// }
|
parseMQTT(readMQTT());
|
||||||
// safety.join();
|
}
|
||||||
// return 0;
|
safety.join();
|
||||||
client.publishMessage("kobuki/data", serializeKobukiData(robot.parser.data));
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string readMQTT()
|
std::string readMQTT()
|
||||||
@@ -200,4 +202,12 @@ std::string serializeKobukiData(const TKobukiData &data) {
|
|||||||
|
|
||||||
json += "]}";
|
json += "]}";
|
||||||
return 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::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||||
|
}
|
||||||
}
|
}
|
Reference in New Issue
Block a user