Currently running on Pi4.
I want to run two threads: one for image recognition using OAK-D and another for UART data transmission.
But no matter how I modify it, there are issues...
The image recognition program using OAK-D works fine when run alone.
main.cpp
#include <iostream>
#include <opencv2/opencv.hpp>
#include "depthai/depthai.hpp"
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <chrono>
#include <fstream>
#include <pthread.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include "oakd_init.h"
#pragma warning(disable:4996)
#define MESSAGE_SIZE 30
int xmin = 0, ymin = 0, xmax = 0;
pthread_mutex_t mutex;
void *serial_thread1(void *arg) {
int fd;
struct termios options;
fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1) {
perror("Unable to open serial port");
pthread_exit(NULL);
}
tcgetattr(fd, &options);
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
tcsetattr(fd, TCSANOW, &options);
struct timespec sleep_time = {0, 16000000L}; // 16ms
while (1) {
pthread_mutex_lock(&mutex);
char message[MESSAGE_SIZE];
sprintf(message, "xmin,ymin,xmax: %3d,%3d,%3d\n", xmin,ymin,xmax);
pthread_mutex_unlock(&mutex);
write(fd, message, strlen(message));
fsync(fd);
nanosleep(&sleep_time, NULL);
}
close(fd);
pthread_exit(NULL);
}
void *serial_thread2(void *arg) {
auto device = static_cast<dai::Device*>(arg);
auto DisparityDataQueue = device->getOutputQueue("DisparityData", 1, false);
auto DetsDataQueue = device->getOutputQueue("DetsData", 1, false);
auto RightDataQueue = device->getOutputQueue("RightData", 1, false);
struct timespec sleep_time = {0, 10000000L}; // 10ms
while (1) {
pthread_mutex_lock(&mutex);
try {
auto DisparityData = DisparityDataQueue->tryGet<dai::ImgFrame>();
auto DetsData = DetsDataQueue->tryGet<dai::NNData>();
auto RightData = RightDataQueue->tryGet<dai::ImgFrame>();
if (DisparityData) {
cv::Mat DisparityFrame = DisparityData->getFrame();
}
if (RightData) {
cv::Mat RightFrame = RightData->getFrame();
}
if (DetsData) {
auto dets = DetsData->getLayerFp16("dets");
int xmin = std::max(0, std::min(static_cast<int>(dets[0] * 640), 640));
int ymin = std::max(0, std::min(static_cast<int>(dets[1] * 400), 400));
int xmax = std::max(0, std::min(static_cast<int>(dets[2] * 640), 640));
int ymax = std::max(0, std::min(static_cast<int>(dets[3] * 400), 400));
printf( "xmin,ymin,xmax : %3d,%3d,%3d\n",xmin,ymin,xmax);
}
} catch (const std::exception& e) {
std::cerr << "Exception in serial_thread2: " << e.what() << std::endl;
}
pthread_mutex_unlock(&mutex);
nanosleep(&sleep_time, NULL);
}
pthread_exit(NULL);
}
int main() {
try {
dai::Pipeline pipeline_oakd;
InitETK_OAKD(pipeline_oakd);
auto device = std::make_shared<dai::Device>(pipeline_oakd, dai::UsbSpeed::SUPER_PLUS);
pthread_t tid1, tid2;
srand(time(NULL));
if (pthread_mutex_init(&mutex, NULL) != 0) {
perror("pthread_mutex_init");
return 1;
}
if (pthread_create(&tid1, NULL, serial_thread1, NULL) != 0) {
perror("pthread_create");
return 1;
}
if (pthread_create(&tid2, NULL, serial_thread2, device.get()) != 0) {
perror("pthread_create");
return 1;
}
pthread_exit(NULL);
} catch (const std::exception& e) {
std::cerr << "Exception: " << e.what() << std::endl;
return 1;
}
}
oakd_init.cpp
#include "depthai/depthai.hpp"
#include "oakd_init.h"
#include <cstdio>
#include <iostream>
using namespace std;
int InitETK_OAKD(dai::Pipeline pipeline_oakd) {
int wr = 320;
int hr = 180;
// Create Mono Camera
printf("[OAK-D] Creating Mono Camera...\n");
auto monoLeft = pipeline_oakd.create<dai::node::MonoCamera>();
monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
monoLeft->setFps(80);
monoLeft->setCamera("left");
auto monoRight = pipeline_oakd.create<dai::node::MonoCamera>();
monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
monoRight->setFps(80);
monoRight->setCamera("right");
//monoRight->initialControl.setManualExposure(12500, 100);
// Creating Stereo Depth
printf("[OAK-D] Creating Stereo Depth...\n");
auto stereoDepth = pipeline_oakd.create<dai::node::StereoDepth>();
monoRight->initialControl.setLumaDenoise(3);
stereoDepth->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
stereoDepth->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
stereoDepth->setLeftRightCheck(true);
stereoDepth->setExtendedDisparity(false);
stereoDepth->setSubpixel(false);
stereoDepth->setDepthAlign(dai::CameraBoardSocket::CAM_C);
stereoDepth->setOutputKeepAspectRatio(true);
auto config = stereoDepth->initialConfig.get();
config.postProcessing.thresholdFilter.minRange = 300;
config.postProcessing.thresholdFilter.maxRange = 2500;
stereoDepth->initialConfig.set(config);
//// Linking
monoLeft->out.link(stereoDepth->left);
monoRight->out.link(stereoDepth->right);
auto manip = pipeline_oakd.create<dai::node::ImageManip>();
manip->initialConfig.setResize(320, 180);
manip->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p);
monoRight->out.link(manip->inputImage);
// Creating Face Detection Neu
printf("[OAK-D] Creating Face Detection Neural Network 1...\n");
auto face_det_nn = pipeline_oakd.create<dai::node::NeuralNetwork>();
face_det_nn->setBlobPath("face_detection.blob");
face_det_nn->input.setQueueSize(1);
face_det_nn->input.setBlocking(false);
manip->out.link(face_det_nn->input);
//// Creating Face Detection Neural Network 2
printf("[OAK-D] Creating Face Detection Neural Network 2 ...\n");
auto postproc_nn = pipeline_oakd.create<dai::node::NeuralNetwork>();
postproc_nn->setBlobPath("postproc.blob");
postproc_nn->input.setQueueSize(1);
postproc_nn->input.setBlocking(false);
face_det_nn->out.link(postproc_nn->input);
// Create XLinkOut
auto xoutDisparityData = pipeline_oakd.create<dai::node::XLinkOut>();
xoutDisparityData->setStreamName("DisparityData");
auto xoutDetsData = pipeline_oakd.create<dai::node::XLinkOut>();
xoutDetsData->setStreamName("DetsData");
auto xoutRightData = pipeline_oakd.create<dai::node::XLinkOut>();
xoutRightData->setStreamName("RightData");
postproc_nn->out.link(xoutDetsData->input);
monoRight->out.link(xoutRightData->input);
stereoDepth->disparity.link(xoutDisparityData->input);
return 0;
}