add pipeline for picam

This commit is contained in:
2024-12-11 15:34:53 +01:00
parent e9f998b3e7
commit 60ba177dc2

View File

@@ -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,11 +31,14 @@ void setup()
int main() int main()
{ {
setup(); setup();
std::thread image (CapnSend); std::thread image(CapnSend);
std::thread safety([&]() { robot.robotSafety(&message); }); std::thread safety([&]()
std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); }); { robot.robotSafety(&message); });
std::thread sendMqtt([&]()
while(true){ { sendKobukiData(robot.parser.data); });
while (true)
{
parseMQTT(readMQTT()); parseMQTT(readMQTT());
} }
@@ -157,8 +160,10 @@ 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));
@@ -204,8 +209,9 @@ 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));
@@ -215,7 +221,8 @@ 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) +
@@ -261,8 +268,9 @@ 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) + "}";
@@ -271,28 +279,34 @@ 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() { {
VideoCapture cap(0, cv::CAP_V4L2); // Open the default camera and set it to the V4L2 backend 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);
if (!cap.isOpened())
{
cerr << "Error: Could not open camera" << endl; cerr << "Error: Could not open camera" << endl;
return; return;
} }
Mat frame; Mat frame;
while (true) { while (true)
{
cap >> frame; // Capture a new image frame cap >> frame; // Capture a new image frame
if (frame.empty()) { if (frame.empty())
{
cerr << "Error: Could not capture image" << endl; cerr << "Error: Could not capture image" << endl;
continue; continue;
} }
@@ -300,7 +314,7 @@ void CapnSend() {
// Convert the image to a byte array // Convert the image to a byte array
vector<uchar> buf; vector<uchar> buf;
imencode(".jpg", frame, buf); imencode(".jpg", frame, buf);
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()));