mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
add pipeline for picam
This commit is contained in:
@@ -32,10 +32,13 @@ int main()
|
||||
{
|
||||
setup();
|
||||
std::thread image(CapnSend);
|
||||
std::thread safety([&]() { robot.robotSafety(&message); });
|
||||
std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
|
||||
std::thread safety([&]()
|
||||
{ robot.robotSafety(&message); });
|
||||
std::thread sendMqtt([&]()
|
||||
{ sendKobukiData(robot.parser.data); });
|
||||
|
||||
while(true){
|
||||
while (true)
|
||||
{
|
||||
parseMQTT(readMQTT());
|
||||
}
|
||||
|
||||
@@ -157,8 +160,10 @@ void logToFile()
|
||||
}
|
||||
}
|
||||
|
||||
void sendIndividualKobukiData(const TKobukiData &data) {
|
||||
while (true) {
|
||||
void sendIndividualKobukiData(const TKobukiData &data)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
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));
|
||||
@@ -204,7 +209,8 @@ void sendIndividualKobukiData(const TKobukiData &data) {
|
||||
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()) {
|
||||
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));
|
||||
@@ -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) +
|
||||
",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
|
||||
",\"BumperLeft\":" + std::to_string(data.BumperLeft) +
|
||||
@@ -261,7 +268,8 @@ std::string serializeKobukiData(const TKobukiData &data) {
|
||||
",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
|
||||
",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
|
||||
|
||||
if (!data.gyroData.empty()) {
|
||||
if (!data.gyroData.empty())
|
||||
{
|
||||
const auto &latestGyro = data.gyroData.back();
|
||||
json += "{\"x\":" + std::to_string(latestGyro.x) +
|
||||
",\"y\":" + std::to_string(latestGyro.y) +
|
||||
@@ -273,26 +281,32 @@ std::string serializeKobukiData(const TKobukiData &data) {
|
||||
}
|
||||
// create extra function to send the message every 100ms
|
||||
// needed it so it can be threaded
|
||||
void sendKobukiData(TKobukiData &data) {
|
||||
while (true) {
|
||||
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, cv::CAP_V4L2); // Open the default camera and set it to the V4L2 backend
|
||||
if (!cap.isOpened()) {
|
||||
void CapnSend()
|
||||
{
|
||||
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())
|
||||
{
|
||||
cerr << "Error: Could not open camera" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
Mat frame;
|
||||
while (true) {
|
||||
while (true)
|
||||
{
|
||||
cap >> frame; // Capture a new image frame
|
||||
if (frame.empty()) {
|
||||
if (frame.empty())
|
||||
{
|
||||
cerr << "Error: Could not capture image" << endl;
|
||||
continue;
|
||||
}
|
||||
|
Reference in New Issue
Block a user