added code from main branch

This commit is contained in:
ishak jmilou.ishak
2024-12-19 20:22:43 +01:00
parent d8ce5de8f7
commit 45247b5574

View File

@@ -3,11 +3,15 @@
#include <thread> #include <thread>
#include "MQTT/MqttClient.h" #include "MQTT/MqttClient.h"
#include "KobukiDriver/CKobuki.h" #include "KobukiDriver/CKobuki.h"
#include <opencv4/opencv2/opencv.hpp>
#include <opencv4/opencv2/core.hpp>
using namespace std; using namespace std;
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();
//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";
@@ -17,7 +21,7 @@ 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
client.connect(); client.connect();
@@ -26,26 +30,33 @@ void setup()
int main() int main()
{ {
// Unset the http_proxy environment variable
setup(); setup();
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); });
while(true){ while(true){
parseMQTT(readMQTT()); std::string message = readMQTT();
if (!message.empty()){
parseMQTT(message);
} }
}
sendMqtt.join(); sendMqtt.join();
safety.join(); safety.join();
image.join();
} }
std::string readMQTT() std::string readMQTT()
{ {
message = client.getLastMessage(); static std::string lastMessage;
if (!message.empty())
std::string message = client.getLastMessage();
if (!message.empty() && message != lastMessage)
{ {
std::cout << "MQTT Message: " << message << std::endl; std::cout << "MQTT Message: " << message << std::endl;
lastMessage = message;
} }
// Add a small delay to avoid busy-waiting // Add a small delay to avoid busy-waiting
@@ -57,7 +68,7 @@ void parseMQTT(std::string message)
{ {
if (message == "up") if (message == "up")
{ {
robot.forward(1024); robot.forward(350);
} }
else if (message == "left") else if (message == "left")
{ {
@@ -69,7 +80,7 @@ void parseMQTT(std::string message)
} }
else if (message == "down") else if (message == "down")
{ {
robot.forward(-800); robot.forward(-350);
} }
else if (message == "stop") else if (message == "stop")
{ {
@@ -276,3 +287,31 @@ void sendKobukiData(TKobukiData &data) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 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) {
cap >> frame; // Capture a new image frame
if (frame.empty()) {
cerr << "Error: Could not capture image" << 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(300)); // Send image every 1000ms
}
}