mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
code revert
This commit is contained in:
@@ -12,7 +12,7 @@ CKobuki robot;
|
|||||||
std::string readMQTT();
|
std::string readMQTT();
|
||||||
void parseMQTT(std::string message);
|
void parseMQTT(std::string message);
|
||||||
void CapnSend();
|
void CapnSend();
|
||||||
// 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);
|
||||||
@@ -22,7 +22,7 @@ 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
|
||||||
|
|
||||||
client.connect();
|
client.connect();
|
||||||
client.subscribe("home/commands");
|
client.subscribe("home/commands");
|
||||||
@@ -31,14 +31,11 @@ void setup()
|
|||||||
int main()
|
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,9 +204,8 @@ 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));
|
||||||
client.publishMessage("kobuki/data/gyroData/z", std::to_string(latestGyro.z));
|
client.publishMessage("kobuki/data/gyroData/z", std::to_string(latestGyro.z));
|
||||||
@@ -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,9 +261,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) +
|
||||||
",\"z\":" + std::to_string(latestGyro.z) + "}";
|
",\"z\":" + std::to_string(latestGyro.z) + "}";
|
||||||
@@ -279,23 +271,21 @@ std::string serializeKobukiData(const TKobukiData &data)
|
|||||||
json += "]}";
|
json += "]}";
|
||||||
return json;
|
return json;
|
||||||
}
|
}
|
||||||
// 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";
|
if (!cap.isOpened()) {
|
||||||
VideoCapture cap(pipeline, cv::CAP_GSTREAMER);
|
cerr << "Error: Could not open camera" << endl;
|
||||||
if (!cap.isOpened()) {
|
|
||||||
cerr << "Error: Could not open camera with GStreamer pipeline" << 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
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Reference in New Issue
Block a user