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 11:55:00 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

refactor: reorder includes and improve code formatting for readability

Browse Source
This commit is contained in:
ishak jmilou.ishak
2025-01-20 11:15:45 +01:00
parent 3d9a68ff7f
commit ef3407b742
1 changed files with 131 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,11 +1,10 @@
#include "KobukiDriver/CKobuki.h"
#include "MQTT/MqttClient.h"
#include <fstream>
#include <iostream> #include <iostream>
#include <opencv4/opencv2/core.hpp>
#include <opencv4/opencv2/opencv.hpp>
#include <thread> #include <thread>
#include <fstream>
#include "MQTT/MqttClient.h"
#include "KobukiDriver/CKobuki.h"
#include <opencv4/opencv2/opencv.hpp>
#include <opencv4/opencv2/core.hpp>
using namespace std; using namespace std;
using namespace cv; using namespace cv;
@@ -17,13 +16,13 @@ void parseMQTT(std::string message);
void CapnSend(); void CapnSend();
void checkKobukiConnection(); void checkKobukiConnection();
// ip, clientID, username, password // ip, clientID, username, password
MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object
"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
@@ -31,17 +30,21 @@ void setup() {
client.subscribe("home/commands"); client.subscribe("home/commands");
} }
int main() { int main()
{
setup(); setup();
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); });
std::thread connectionChecker(checkKobukiConnection); std::thread connectionChecker(checkKobukiConnection);
connectionChecker.detach(); // Laat deze thread onafhankelijk draaien connectionChecker.detach(); // Laat deze thread onafhankelijk draaien
while (true) { while (true)
{
std::string message = readMQTT(); std::string message = readMQTT();
if (!message.empty()) { if (!message.empty())
{
parseMQTT(message); parseMQTT(message);
} }
} }
@@ -53,42 +56,52 @@ int main() {
std::mutex connectionMutex; std::mutex connectionMutex;
void checkKobukiConnection() { void checkKobukiConnection()
while (true) { {
while (true)
{ {
std::lock_guard<std::mutex> lock(connectionMutex); {
if (!robot.isConnected()) { std::lock_guard<std::mutex> lock(connectionMutex);
if (kobuki_connected) { if (!robot.isConnected())
cout << "Kobuki is disconnected" << endl; {
kobuki_connected = false; if (kobuki_connected)
{
cout << "Kobuki is disconnected" << endl;
kobuki_connected = false;
}
// Probeer opnieuw te verbinden
cout << "Attempting to reconnect Kobuki..." << endl;
robot.startCommunication("/dev/ttyUSB0", true, nullptr);
kobuki_connected = robot.isConnected();
if (kobuki_connected)
{
cout << "Kobuki reconnected successfully!" << endl;
}
else
{
cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl;
}
}else{
if (!kobuki_connected)
{
cout << "Kobuki is connected" << endl;
kobuki_connected = true;
}
} }
// Probeer opnieuw te verbinden std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
cout << "Attempting to reconnect Kobuki..." << endl;
robot.startCommunication("/dev/ttyUSB0", true, nullptr);
kobuki_connected = robot.isConnected();
if (kobuki_connected) {
cout << "Kobuki reconnected successfully!" << endl;
} else {
cout << "Failed to reconnect Kobuki, retrying in 5 seconds..."
<< endl;
}
} else {
if (!kobuki_connected) {
cout << "Kobuki is connected" << endl;
kobuki_connected = true;
}
}
std::this_thread::sleep_for(
std::chrono::seconds(5)); // Controleer elke 5 seconden
} }
} }
} }
std::string readMQTT() {
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;
} }
@@ -98,32 +111,49 @@ std::string readMQTT() {
return lastMessage; return lastMessage;
} }
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::ofstream outputFile("log",
std::ios_base::app); // Open file in append mode to std::ios_base::app); // Open file in append mode to
// not overwrite own content // not overwrite own content
if (outputFile.is_open()) { // check if the file was opened successfully 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);
@@ -181,7 +211,9 @@ void logToFile() {
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";
} }
@@ -189,8 +221,10 @@ 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: " std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: "
<< data.timestamp << std::endl; << data.timestamp << std::endl;
client.publishMessage("kobuki/data/timestamp", client.publishMessage("kobuki/data/timestamp",
@@ -278,7 +312,8 @@ void sendIndividualKobukiData(const TKobukiData &data) {
client.publishMessage("kobuki/data/extraInfo/UDID2", client.publishMessage("kobuki/data/extraInfo/UDID2",
std::to_string(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", client.publishMessage("kobuki/data/gyroData/x",
std::to_string(latestGyro.x)); std::to_string(latestGyro.x));
@@ -292,7 +327,8 @@ void sendIndividualKobukiData(const TKobukiData &data) {
} }
} }
std::string serializeKobukiData(const TKobukiData &data) { std::string serializeKobukiData(const TKobukiData &data)
{
std::string json = std::string json =
"{\"timestamp\":" + std::to_string(data.timestamp) + "{\"timestamp\":" + std::to_string(data.timestamp) +
",\"BumperCenter\":" + std::to_string(data.BumperCenter) + ",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
@@ -345,7 +381,8 @@ std::string serializeKobukiData(const TKobukiData &data) {
",\"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\":[";
if (!data.gyroData.empty()) { if (!data.gyroData.empty())
{
const auto &latestGyro = data.gyroData.back(); const auto &latestGyro = data.gyroData.back();
json += "{\"x\":" + std::to_string(latestGyro.x) + json += "{\"x\":" + std::to_string(latestGyro.x) +
",\"y\":" + std::to_string(latestGyro.y) + ",\"y\":" + std::to_string(latestGyro.y) +
@@ -357,8 +394,10 @@ std::string serializeKobukiData(const TKobukiData &data) {
} }
// create extra function to send the message every 100ms // create extra function to send the message every 100ms
// needed it so it can be threaded // needed it so it can be threaded
void sendKobukiData(TKobukiData &data) { void sendKobukiData(TKobukiData &data)
while (true) { {
while (true)
{
client.publishMessage("kobuki/data", serializeKobukiData(data)); client.publishMessage("kobuki/data", serializeKobukiData(data));
std::cout << "Sent data" << std::endl; std::cout << "Sent data" << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::this_thread::sleep_for(std::chrono::milliseconds(1000));
@@ -366,41 +405,39 @@ void sendKobukiData(TKobukiData &data) {
} }
void CapnSend() { void CapnSend() {
VideoCapture cap(0); VideoCapture cap(0);
if (!cap.isOpened()) { if (!cap.isOpened()) {
cerr << "Error: Could not open camera" << endl; cerr << "Error: Could not open camera" << endl;
return; return;
}
Mat frame;
while (true) {
if (!cap.read(frame)) {
cout << "Reconnecting camera" << endl;
cap.release();
std::this_thread::sleep_for(std::chrono::seconds(1));
// Attempt to reconnect to the camera
cap.open(0);
if (!cap.isOpened()) {
cerr << "Error: Could not reconnect to camera" << endl;
std::this_thread::sleep_for(
std::chrono::seconds(1)); // Wait before retrying
continue;
} else {
cout << "Reconnected to camera" << endl;
continue;
}
} }
// Convert the image to a byte array Mat frame;
vector<uchar> buf; while (true) {
imencode(".jpg", frame, buf); if (!cap.read(frame)) {
auto *enc_msg = reinterpret_cast<unsigned char *>(buf.data()); cout << "Reconnecting camera" << endl;
cap.release();
std::this_thread::sleep_for(std::chrono::seconds(1));
// Attempt to reconnect to the camera
cap.open(0);
if (!cap.isOpened()) {
cerr << "Error: Could not reconnect to camera" << endl;
std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait before retrying
continue;
} else {
cout << "Reconnected to camera" << endl;
continue;
}
}
// Publish the image data // Convert the image to a byte array
client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size())); vector<uchar> buf;
cout << "Sent image" << endl; imencode(".jpg", frame, buf);
auto *enc_msg = reinterpret_cast<unsigned char *>(buf.data());
std::this_thread::sleep_for( // Publish the image data
std::chrono::milliseconds(200)); // Send image every 200ms client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
} cout << "Sent image" << endl;
std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Send image every 200ms
}
} }