Smikkelbakje/J2S1-Kobuki
1
0
Fork 0
mirror of https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git synced 2025-08-03 20:04:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

functions for auto reconnect with pi and kobuki

Browse Source
This commit is contained in:
ishak jmilou.ishak
2025-01-07 14:28:06 +01:00
parent bb1904b125
commit b111e49dff
3 changed files with 345 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/C++/Driver/src/KobukiDriver/CKobuki.cpp
View File

@@ -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 *CKobuki::readKobukiMessage() {
unsigned char buffer[1]; unsigned char buffer[1];
ssize_t Pocet; ssize_t Pocet;

1
src/C++/Driver/src/KobukiDriver/CKobuki.h
View File

@@ -80,6 +80,7 @@ public:
void robotSafety(); //overload void robotSafety(); //overload
void sendNullMessage(); void sendNullMessage();
bool safetyActive = false; bool safetyActive = false;
bool isConnected();
KobukiParser parser; KobukiParser parser;

283
src/C++/Driver/src/main.cpp
View File

@@ -1,25 +1,28 @@
#include <iostream>
#include <cmath>
#include <thread>
#include "MQTT/MqttClient.h"
#include "KobukiDriver/CKobuki.h" #include "KobukiDriver/CKobuki.h"
#include <opencv4/opencv2/opencv.hpp> #include "MQTT/MqttClient.h"
#include <cmath>
#include <iostream>
#include <opencv4/opencv2/core.hpp> #include <opencv4/opencv2/core.hpp>
#include <opencv4/opencv2/opencv.hpp>
#include <thread>
using namespace std; using namespace std;
using namespace cv; using namespace cv;
CKobuki robot; CKobuki robot;
std::string readMQTT(); std::string readMQTT();
void parseMQTT(std::string message); void parseMQTT(std::string message);
void CapnSend(); void CapnSend();
void reconnectKobuki();
void monitorKobukiConnection();
// ip, clientID, username, password // ip, clientID, username, password
MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi",
"rpiwachtwoordofzo"); // 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 sendKobukiData(TKobukiData &data);
void setup() void setup() {
{
unsigned char *null_ptr(0); unsigned char *null_ptr(0);
robot.startCommunication("/dev/ttyUSB0", true, null_ptr); robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
// connect mqtt server and sub to commands // connect mqtt server and sub to commands
@@ -28,9 +31,11 @@ void setup()
client.subscribe("home/commands"); client.subscribe("home/commands");
} }
int main() int main() {
{
setup(); setup();
reconnectKobuki();
std::thread connectionMonitor(monitorKobukiConnection);
std::thread image(CapnSend); std::thread image(CapnSend);
std::thread safety([&]() { robot.robotSafety(&message); }); std::thread safety([&]() { robot.robotSafety(&message); });
std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); }); std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
@@ -40,7 +45,6 @@ int main()
if (!message.empty()) { if (!message.empty()) {
parseMQTT(message); parseMQTT(message);
} }
} }
sendMqtt.join(); sendMqtt.join();
@@ -48,13 +52,35 @@ int main()
image.join(); image.join();
} }
std::string readMQTT() 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));
}
}
}
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
}
}
std::string readMQTT() {
static std::string lastMessage; static std::string lastMessage;
std::string message = client.getLastMessage(); std::string message = client.getLastMessage();
if (!message.empty() && message != lastMessage) if (!message.empty() && message != lastMessage) {
{
std::cout << "MQTT Message: " << message << std::endl; std::cout << "MQTT Message: " << message << std::endl;
lastMessage = message; lastMessage = message;
} }
@@ -64,47 +90,32 @@ std::string readMQTT()
return message; return message;
} }
void parseMQTT(std::string message) void parseMQTT(std::string message) {
{ if (message == "up") {
if (message == "up")
{
robot.forward(350); robot.forward(350);
} } else if (message == "left") {
else if (message == "left")
{
robot.setRotationSpeed(4); robot.setRotationSpeed(4);
} } else if (message == "right") {
else if (message == "right")
{
robot.setRotationSpeed(-4); robot.setRotationSpeed(-4);
} } else if (message == "down") {
else if (message == "down")
{
robot.forward(-350); robot.forward(-350);
} } else if (message == "stop") {
else if (message == "stop")
{
robot.sendNullMessage(); robot.sendNullMessage();
robot.sendNullMessage(); robot.sendNullMessage();
} } else if (message == "estop") {
else if (message == "estop")
{
robot.forward(-400); robot.forward(-400);
} } else {
else
{
std::cout << "Invalid command" << std::endl; std::cout << "Invalid command" << std::endl;
} }
} }
void logToFile() void logToFile() {
{ while (true) {
while (true)
{
TKobukiData robotData = robot.parser.data; TKobukiData robotData = robot.parser.data;
std::ofstream outputFile("log", std::ios_base::app); // Open file in append mode to not overwrite own content std::ofstream outputFile("log",
if (outputFile.is_open()) std::ios_base::app); // Open file in append mode to
{ // check if the file was opened successfully // not overwrite own content
if (outputFile.is_open()) { // check if the file was opened successfully
// Get current time // Get current time
std::time_t now = std::time(nullptr); std::time_t now = std::time(nullptr);
outputFile << "Timestamp: " << std::ctime(&now); outputFile << "Timestamp: " << std::ctime(&now);
@@ -139,24 +150,30 @@ void logToFile()
outputFile << "GyroAngle: " << robotData.GyroAngle << "\n"; outputFile << "GyroAngle: " << robotData.GyroAngle << "\n";
outputFile << "GyroAngleRate: " << robotData.GyroAngleRate << "\n"; outputFile << "GyroAngleRate: " << robotData.GyroAngleRate << "\n";
outputFile << "CliffSensorRight: " << robotData.CliffSensorRight << "\n"; outputFile << "CliffSensorRight: " << robotData.CliffSensorRight << "\n";
outputFile << "CliffSensorCenter: " << robotData.CliffSensorCenter << "\n"; outputFile << "CliffSensorCenter: " << robotData.CliffSensorCenter
<< "\n";
outputFile << "CliffSensorLeft: " << robotData.CliffSensorLeft << "\n"; outputFile << "CliffSensorLeft: " << robotData.CliffSensorLeft << "\n";
outputFile << "wheelCurrentLeft: " << robotData.wheelCurrentLeft << "\n"; outputFile << "wheelCurrentLeft: " << robotData.wheelCurrentLeft << "\n";
outputFile << "wheelCurrentRight: " << robotData.wheelCurrentRight << "\n"; outputFile << "wheelCurrentRight: " << robotData.wheelCurrentRight
<< "\n";
outputFile << "frameId: " << robotData.frameId << "\n"; outputFile << "frameId: " << robotData.frameId << "\n";
outputFile << "HardwareVersionPatch: " << robotData.extraInfo.HardwareVersionPatch << "\n"; outputFile << "HardwareVersionPatch: "
outputFile << "HardwareVersionMinor: " << robotData.extraInfo.HardwareVersionMinor << "\n"; << robotData.extraInfo.HardwareVersionPatch << "\n";
outputFile << "HardwareVersionMajor: " << robotData.extraInfo.HardwareVersionMajor << "\n"; outputFile << "HardwareVersionMinor: "
outputFile << "FirmwareVersionPatch: " << robotData.extraInfo.FirmwareVersionPatch << "\n"; << robotData.extraInfo.HardwareVersionMinor << "\n";
outputFile << "FirmwareVersionMinor: " << robotData.extraInfo.FirmwareVersionMinor << "\n"; outputFile << "HardwareVersionMajor: "
outputFile << "FirmwareVersionMajor: " << robotData.extraInfo.FirmwareVersionMajor << "\n"; << 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 << "UDID0: " << robotData.extraInfo.UDID0 << "\n";
outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n"; outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n";
outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n"; outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n";
outputFile.close(); outputFile.close();
} } else {
else
{
std::cerr << "Error opening file\n"; std::cerr << "Error opening file\n";
} }
@@ -166,57 +183,101 @@ void logToFile()
void sendIndividualKobukiData(const TKobukiData &data) { void sendIndividualKobukiData(const TKobukiData &data) {
while (true) { while (true) {
std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: " << data.timestamp << std::endl; std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: "
client.publishMessage("kobuki/data/timestamp", std::to_string(data.timestamp)); << data.timestamp << std::endl;
client.publishMessage("kobuki/data/BumperCenter", std::to_string(data.BumperCenter)); client.publishMessage("kobuki/data/timestamp",
client.publishMessage("kobuki/data/BumperLeft", std::to_string(data.BumperLeft)); std::to_string(data.timestamp));
client.publishMessage("kobuki/data/BumperRight", std::to_string(data.BumperRight)); client.publishMessage("kobuki/data/BumperCenter",
client.publishMessage("kobuki/data/WheelDropLeft", std::to_string(data.WheelDropLeft)); std::to_string(data.BumperCenter));
client.publishMessage("kobuki/data/WheelDropRight", std::to_string(data.WheelDropRight)); client.publishMessage("kobuki/data/BumperLeft",
client.publishMessage("kobuki/data/CliffCenter", std::to_string(data.CliffCenter)); std::to_string(data.BumperLeft));
client.publishMessage("kobuki/data/CliffLeft", std::to_string(data.CliffLeft)); client.publishMessage("kobuki/data/BumperRight",
client.publishMessage("kobuki/data/CliffRight", std::to_string(data.CliffRight)); std::to_string(data.BumperRight));
client.publishMessage("kobuki/data/EncoderLeft", std::to_string(data.EncoderLeft)); client.publishMessage("kobuki/data/WheelDropLeft",
client.publishMessage("kobuki/data/EncoderRight", std::to_string(data.EncoderRight)); 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/PWMleft", std::to_string(data.PWMleft));
client.publishMessage("kobuki/data/PWMright", std::to_string(data.PWMright)); client.publishMessage("kobuki/data/PWMright",
client.publishMessage("kobuki/data/ButtonPress1", std::to_string(data.ButtonPress1)); std::to_string(data.PWMright));
client.publishMessage("kobuki/data/ButtonPress2", std::to_string(data.ButtonPress2)); client.publishMessage("kobuki/data/ButtonPress1",
client.publishMessage("kobuki/data/ButtonPress3", std::to_string(data.ButtonPress3)); 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/Charger", std::to_string(data.Charger));
client.publishMessage("kobuki/data/Battery", std::to_string(data.Battery)); client.publishMessage("kobuki/data/Battery", std::to_string(data.Battery));
client.publishMessage("kobuki/data/overCurrent", std::to_string(data.overCurrent)); client.publishMessage("kobuki/data/overCurrent",
client.publishMessage("kobuki/data/IRSensorRight", std::to_string(data.IRSensorRight)); std::to_string(data.overCurrent));
client.publishMessage("kobuki/data/IRSensorCenter", std::to_string(data.IRSensorCenter)); client.publishMessage("kobuki/data/IRSensorRight",
client.publishMessage("kobuki/data/IRSensorLeft", std::to_string(data.IRSensorLeft)); std::to_string(data.IRSensorRight));
client.publishMessage("kobuki/data/GyroAngle", std::to_string(data.GyroAngle)); client.publishMessage("kobuki/data/IRSensorCenter",
client.publishMessage("kobuki/data/GyroAngleRate", std::to_string(data.GyroAngleRate)); std::to_string(data.IRSensorCenter));
client.publishMessage("kobuki/data/CliffSensorRight", std::to_string(data.CliffSensorRight)); client.publishMessage("kobuki/data/IRSensorLeft",
client.publishMessage("kobuki/data/CliffSensorCenter", std::to_string(data.CliffSensorCenter)); std::to_string(data.IRSensorLeft));
client.publishMessage("kobuki/data/CliffSensorLeft", std::to_string(data.CliffSensorLeft)); client.publishMessage("kobuki/data/GyroAngle",
client.publishMessage("kobuki/data/wheelCurrentLeft", std::to_string(data.wheelCurrentLeft)); std::to_string(data.GyroAngle));
client.publishMessage("kobuki/data/wheelCurrentRight", std::to_string(data.wheelCurrentRight)); client.publishMessage("kobuki/data/GyroAngleRate",
client.publishMessage("kobuki/data/digitalInput", std::to_string(data.digitalInput)); std::to_string(data.GyroAngleRate));
client.publishMessage("kobuki/data/analogInputCh0", std::to_string(data.analogInputCh0)); client.publishMessage("kobuki/data/CliffSensorRight",
client.publishMessage("kobuki/data/analogInputCh1", std::to_string(data.analogInputCh1)); std::to_string(data.CliffSensorRight));
client.publishMessage("kobuki/data/analogInputCh2", std::to_string(data.analogInputCh2)); client.publishMessage("kobuki/data/CliffSensorCenter",
client.publishMessage("kobuki/data/analogInputCh3", std::to_string(data.analogInputCh3)); 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/frameId", std::to_string(data.frameId));
client.publishMessage("kobuki/data/extraInfo/HardwareVersionPatch", std::to_string(data.extraInfo.HardwareVersionPatch)); client.publishMessage("kobuki/data/extraInfo/HardwareVersionPatch",
client.publishMessage("kobuki/data/extraInfo/HardwareVersionMinor", std::to_string(data.extraInfo.HardwareVersionMinor)); std::to_string(data.extraInfo.HardwareVersionPatch));
client.publishMessage("kobuki/data/extraInfo/HardwareVersionMajor", std::to_string(data.extraInfo.HardwareVersionMajor)); client.publishMessage("kobuki/data/extraInfo/HardwareVersionMinor",
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionPatch", std::to_string(data.extraInfo.FirmwareVersionPatch)); std::to_string(data.extraInfo.HardwareVersionMinor));
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMinor", std::to_string(data.extraInfo.FirmwareVersionMinor)); client.publishMessage("kobuki/data/extraInfo/HardwareVersionMajor",
client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMajor", std::to_string(data.extraInfo.FirmwareVersionMajor)); std::to_string(data.extraInfo.HardwareVersionMajor));
client.publishMessage("kobuki/data/extraInfo/UDID0", std::to_string(data.extraInfo.UDID0)); client.publishMessage("kobuki/data/extraInfo/FirmwareVersionPatch",
client.publishMessage("kobuki/data/extraInfo/UDID1", std::to_string(data.extraInfo.UDID1)); std::to_string(data.extraInfo.FirmwareVersionPatch));
client.publishMessage("kobuki/data/extraInfo/UDID2", std::to_string(data.extraInfo.UDID2)); 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()) { if (!data.gyroData.empty()) {
const auto &latestGyro = data.gyroData.back(); const auto &latestGyro = data.gyroData.back();
client.publishMessage("kobuki/data/gyroData/x", std::to_string(latestGyro.x)); client.publishMessage("kobuki/data/gyroData/x",
client.publishMessage("kobuki/data/gyroData/y", std::to_string(latestGyro.y)); std::to_string(latestGyro.x));
client.publishMessage("kobuki/data/gyroData/z", std::to_string(latestGyro.z)); 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::this_thread::sleep_for(std::chrono::milliseconds(1000));
@@ -224,7 +285,8 @@ void sendIndividualKobukiData(const TKobukiData &data) {
} }
std::string serializeKobukiData(const TKobukiData &data) { std::string serializeKobukiData(const TKobukiData &data) {
std::string json = "{\"timestamp\":" + std::to_string(data.timestamp) + std::string json =
"{\"timestamp\":" + std::to_string(data.timestamp) +
",\"BumperCenter\":" + std::to_string(data.BumperCenter) + ",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
",\"BumperLeft\":" + std::to_string(data.BumperLeft) + ",\"BumperLeft\":" + std::to_string(data.BumperLeft) +
",\"BumperRight\":" + std::to_string(data.BumperRight) + ",\"BumperRight\":" + std::to_string(data.BumperRight) +
@@ -259,12 +321,18 @@ std::string serializeKobukiData(const TKobukiData &data) {
",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) + ",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) +
",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) + ",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) +
",\"frameId\":" + std::to_string(data.frameId) + ",\"frameId\":" + std::to_string(data.frameId) +
",\"extraInfo\":{\"HardwareVersionPatch\":" + std::to_string(data.extraInfo.HardwareVersionPatch) + ",\"extraInfo\":{\"HardwareVersionPatch\":" +
",\"HardwareVersionMinor\":" + std::to_string(data.extraInfo.HardwareVersionMinor) + std::to_string(data.extraInfo.HardwareVersionPatch) +
",\"HardwareVersionMajor\":" + std::to_string(data.extraInfo.HardwareVersionMajor) + ",\"HardwareVersionMinor\":" +
",\"FirmwareVersionPatch\":" + std::to_string(data.extraInfo.FirmwareVersionPatch) + std::to_string(data.extraInfo.HardwareVersionMinor) +
",\"FirmwareVersionMinor\":" + std::to_string(data.extraInfo.FirmwareVersionMinor) + ",\"HardwareVersionMajor\":" +
",\"FirmwareVersionMajor\":" + std::to_string(data.extraInfo.FirmwareVersionMajor) + 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) + ",\"UDID0\":" + std::to_string(data.extraInfo.UDID0) +
",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) + ",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":["; ",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
@@ -313,6 +381,7 @@ void CapnSend() {
client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size())); client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
cout << "Sent image" << endl; 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
} }
} }