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

code revert

Browse Source
This commit is contained in:
Sam Hos
2024-12-11 16:07:26 +01:00
parent 0dfc3b5c13
commit e682969ec8
1 changed files with 26 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -32,13 +32,10 @@ int main()
{ {
setup(); setup();
std::thread image (CapnSend); std::thread image (CapnSend);
std::thread safety([&]() std::thread safety([&]() { robot.robotSafety(&message); });
{ robot.robotSafety(&message); }); std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
std::thread sendMqtt([&]()
{ sendKobukiData(robot.parser.data); });
while (true) while(true){
{
parseMQTT(readMQTT()); parseMQTT(readMQTT());
} }
@@ -160,10 +157,8 @@ void logToFile()
} }
} }
void sendIndividualKobukiData(const TKobukiData &data) void sendIndividualKobukiData(const TKobukiData &data) {
{ while (true) {
while (true)
{
client.publishMessage("kobuki/data/timestamp", std::to_string(data.timestamp)); client.publishMessage("kobuki/data/timestamp", std::to_string(data.timestamp));
client.publishMessage("kobuki/data/BumperCenter", std::to_string(data.BumperCenter)); client.publishMessage("kobuki/data/BumperCenter", std::to_string(data.BumperCenter));
client.publishMessage("kobuki/data/BumperLeft", std::to_string(data.BumperLeft)); client.publishMessage("kobuki/data/BumperLeft", std::to_string(data.BumperLeft));
@@ -209,8 +204,7 @@ void sendIndividualKobukiData(const TKobukiData &data)
client.publishMessage("kobuki/data/extraInfo/UDID1", std::to_string(data.extraInfo.UDID1)); client.publishMessage("kobuki/data/extraInfo/UDID1", std::to_string(data.extraInfo.UDID1));
client.publishMessage("kobuki/data/extraInfo/UDID2", std::to_string(data.extraInfo.UDID2)); 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", std::to_string(latestGyro.x));
client.publishMessage("kobuki/data/gyroData/y", std::to_string(latestGyro.y)); client.publishMessage("kobuki/data/gyroData/y", std::to_string(latestGyro.y));
@@ -221,8 +215,7 @@ 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) +
@@ -268,8 +261,7 @@ 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) +
@@ -281,21 +273,19 @@ 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));
} }
} }
void CapnSend() { void CapnSend() {
// Use GStreamer pipeline to access the PiCam VideoCapture cap(0);
std::string pipeline = "libcamerasrc ! video/x-raw,width=640,height=480,framerate=30/1 ! videoconvert ! appsink";
VideoCapture cap(pipeline, cv::CAP_GSTREAMER);
if (!cap.isOpened()) { if (!cap.isOpened()) {
cerr << "Error: Could not open camera with GStreamer pipeline" << endl; cerr << "Error: Could not open camera" << endl;
return; return;
} }
@@ -313,9 +303,8 @@ void CapnSend() {
auto* enc_msg = reinterpret_cast<unsigned char*>(buf.data()); auto* enc_msg = reinterpret_cast<unsigned char*>(buf.data());
// Publish the image data // Publish the image data
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(400)); // Send image every 1000ms
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Send image every 1000ms
} }
} }