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
Files
ec44cb955b6a17ca258bbcd3c6f7b5ee612d56e5
J2S1-Kobuki/src/C++/Driver/src/main.cpp

458 lines
19 KiB
C++
Raw Blame History

#include <iostream>
#include <thread>
#include <fstream>
#include <filesystem>
#include "MQTT/MqttClient.h"
#include "KobukiDriver/CKobuki.h"
#include <opencv4/opencv2/opencv.hpp>
#include <opencv4/opencv2/core.hpp>
using namespace std;
using namespace cv;
CKobuki robot;
std::atomic<bool> kobuki_connected(false);
std::string readMQTT();
void parseMQTT(std::string message);
void CapnSend();
void checkKobukiConnection();
// 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);
std::string findKobukiPort()
{
for (const auto& entry : std::filesystem::directory_iterator("/dev"))
{
std::string device = entry.path().string();
if (device.find("ttyUSB") != std::string::npos)
{
return device; // Returneer de eerste gevonden poort
}
}
return ""; // Geen poort gevonden
}
void setup()
{
std::string port = findKobukiPort();
unsigned char *null_ptr(0);
robot.startCommunication(const_cast<char*>(port.c_str()), 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); });
std::thread connectionChecker(checkKobukiConnection);
connectionChecker.detach(); // Laat deze thread onafhankelijk draaien
while (true)
{
std::string message = readMQTT();
if (!message.empty())
{
parseMQTT(message);
}
}
sendMqtt.join();
safety.join();
image.join();
}
std::mutex connectionMutex;
void checkKobukiConnection()
{
while (true)
{
std::lock_guard<std::mutex> lock(connectionMutex);
std::string port = findKobukiPort();
if (port.empty()) {
if (kobuki_connected) {
cout << "Kobuki disconnected: No USB device found." << endl;
kobuki_connected = false;
}
std::this_thread::sleep_for(std::chrono::seconds(5));
continue; // Probeer later opnieuw
}
// Controleer of de Kobuki verbonden is
if (!robot.isConnected()){
if (kobuki_connected){
cout << "Kobuki disconnected." << endl;
kobuki_connected = false;
}
cout << "Attempting to reconnect Kobuki..." << endl;
robot.startCommunication(const_cast<char*>(port.c_str()), true, nullptr);
if (robot.isConnected()){
cout << "Kobuki reconnected successfully!" << endl;
kobuki_connected = true;
}
else{
cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl;
}
}
// Wacht voordat je opnieuw controleert
std::this_thread::sleep_for(std::chrono::seconds(5));
}
}
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 lastMessage;
}
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)
{
std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: "
<< data.timestamp << std::endl;
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));
}
}
void CapnSend() {
VideoCapture cap(0);
if (!cap.isOpened()) {
cerr << "Error: Could not open camera" << endl;
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
vector<uchar> buf;
imencode(".jpg", frame, buf);
auto *enc_msg = reinterpret_cast<unsigned char *>(buf.data());
// Publish the image data
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
}
}
View Git Blame Copy Permalink