Ressource guarding multithreading

AutonomousMode/autonomous_mode_main.cpp

@@ -4,19 +4,52 @@
 int main(void)
 {
     //Disable opencv logging messages
-    cv::utils::logging::setLogLevel(cv::utils::logging::LOG_LEVEL_WARNING);
+    //cv::utils::logging::setLogLevel(cv::utils::logging::LOG_LEVEL_WARNING);
 
     const int thresholdBinary = 140;
     const int videoHeight = 720;
     const int videoWidth = 1280;
     const int gaussKernelSize = 11;
 
-    LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize);
+    std::mutex mutex;
-    lfr.saveOutputFlag = false;
+
-    lfr.videoFlag = true;
+    LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        std::cerr<<"camera exception:"<<ex.what()<<std::endl;
+        return true;
+    });
+
+    //To calculate the frame rate
+    std::chrono::milliseconds last = duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+    std::chrono::milliseconds now = duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+    
+    cv::Mat img;
+    lfr.addListener([&](LFR_Result result)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        if (!result.rawImage.empty())
+        {
+            cv::resize(result.rawImage, img, cv::Size(128, 64+32));
+
+            //Calculate frame rate
+            now = duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+            unsigned int deltaMs = (now-last).count();
+            float delta = static_cast<float>(deltaMs) / 1000.0;
+            float frameRate = 1.0 / static_cast<float>(delta);
+
+            //std::cout << "Frame rate: " << frameRate << std::endl;
+            last = now;
+        }
+    }, &mutex);
+
     lfr.startLoop();
-    //To end the video stream, write any char in the console.
+    
-    char a;
+    for(int finished = false; finished != 'q';){
-    std::cin >> a; 
+        finished = std::tolower(cv::waitKey(66));
-    lfr.endLoop();
+        std::unique_lock<std::mutex> lock(mutex);
+        if(!img.empty()){
+            cv::imshow("frame", img);
+        }
+    }
 }

AutonomousMode/lfr.cpp

@@ -3,63 +3,114 @@
 #define right false
 #define left true
 
-LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize)
+LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, ExceptionCallback cb):
-    : iAmLooping(false), input(videoHeight, videoWidth), processing(), controlModule(), interpreter(), intersectionHandler()
+    stop(false),
+    input(videoHeight, videoWidth),
+    processing(),
+    controlModule(),
+    interpreter(),
+    intersectionHandler(),
+    cb(cb)
 {
-    this->iAmLooping = false;
     this->thresholdBinary = thresholdBinary;
     this->gaussKernelSize = gaussKernelSize;
-
-    this->videoFlag = false;
-    this->saveOutputFlag = false;
-    this->outputFileName = "";
 }
 
 LFR::~LFR()
 {
-    if(iAmLooping)
+    endLoop();
+    thread->join();
+}
+
+void LFR::removeListener(LFR::ListenerKey key)
+{
+    std::lock_guard<std::mutex> lock(mutex);
+    auto it = std::find_if(std::begin(listeners), std::end(listeners), [&](auto const &val){
+        return val.first == key;
+    });
+    if(it != std::end(listeners))
     {
-        this->endLoop();
+        listeners.erase(it);
     }
 }
 
-void LFR::loop()
+void LFR::addListener(LFR::ListenerCallback cb, LFR::ListenerKey key)
 {
-    if(this->videoFlag) {namedWindow("Display window");}
+    std::lock_guard<std::mutex> lock(mutex);
-    while(iAmLooping)
+    listeners.emplace_back(key, std::move(cb));
-    {
+}
-        Mat originalImage = input.readWebcam();
 
-        Point roiOrigin(0, int(originalImage.rows*(3.25/6.0)));
+void LFR::setStop(bool val)
-        Rect roi(roiOrigin.x, roiOrigin.y, originalImage.cols, originalImage.rows/6);
+{
-        //Mat processedImage = originalImage(roi);
+    std::lock_guard<std::mutex> lock(mutex);
-        Mat processedImage = originalImage;
+    stop = val;
+}
 
-        processing.processImage(processedImage, this->thresholdBinary, this->gaussKernelSize);
+void LFR::createThread()
-        //processedImage = processedImage(roi);
+{
-        FrameData data = processing.calculateLineSegments(processedImage, roi);
+    thread = std::make_unique<std::thread>([this](){
-        processing.filterReflections(data);
+        while(true)
-        processing.calcAngles(data, originalImage.cols, originalImage.rows, left);
+        {
-        this->provideOutput(originalImage, processedImage, data, roi);
+            LFR_Result result;
-    }
+            if(!stop && !listeners.empty())
-    if(this->videoFlag) {destroyWindow("Display window");}
+            {
-    input.freeWebcam();
+                try
+                {
+                    std::lock_guard<std::mutex> lock(mutex);
+                    Mat originalImage = input.readWebcam();
+
+                    Point roiOrigin(0, int(originalImage.rows*(3.25/6.0)));
+                    Rect roi(roiOrigin.x, roiOrigin.y, originalImage.cols, originalImage.rows/6);
+                    Mat processedImage = originalImage;
+
+                    processing.processImage(processedImage, this->thresholdBinary, this->gaussKernelSize);
+                    FrameData data = processing.calculateLineSegments(processedImage, roi);
+                    processing.filterReflections(data);
+                    processing.calcAngles(data, originalImage.cols, originalImage.rows, left);
+
+                    processedImage = provideOutput(originalImage, processedImage, data, roi);
+
+                    result.rawImage = originalImage;
+                    result.processedImage = processedImage;
+                    result.data = data;
+                }
+                catch(std::exception const &ex)
+                {
+                    cb(ex);
+                }
+
+                //Invoke the callback method (ListenerPair second -> ListenerCallback)
+                for(auto &val : listeners)
+                {
+                    val.second(result);
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+    });
 }
 
 void LFR::startLoop()
 {
-    iAmLooping = true;
+    if(thread)
-    this->loopThread=thread(&LFR::loop, this);
+    {
+        //Restart thread if it is running
+        setStop(true);
+        thread->join();
+        setStop(false);
+    }
+    createThread();
 }
 
 void LFR::endLoop()
 {
-    iAmLooping = false;
+    setStop(true);
-    this->loopThread.join();
-    return;
 }
 
-void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi)
+cv::Mat LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi)
 {
     for(int i = 0; i < frameData.contours.size(); i++)
     {
@@ -83,14 +134,5 @@ void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData&
         P2.y =  (int)round(P1.y + length * sin(frameData.angle * CV_PI / 180.0));
         cv::arrowedLine(originalImage, P1, P2, Scalar(0,0,255), 2, 8);
     }
-    if(this->videoFlag)
+    return originalImage;
-    {
-        imshow("Display window", originalImage);
-        imshow("processed:", processedImage);
-        char c = (char)waitKey(25);
-    }
-    if (this->saveOutputFlag && !(this->outputFileName.empty()))
-    {
-        imwrite(this->outputFileName, originalImage);        
-    }
 }

AutonomousMode/lfr.h

@@ -3,6 +3,7 @@
 #include <iostream>
 #include <future>
 #include <thread>
+#include <functional>
 
 #include <opencv2/opencv.hpp>
 
@@ -15,34 +16,52 @@
 
 using namespace cv;
 
+struct LFR_Result
+{
+    cv::Mat rawImage;
+    cv::Mat processedImage;
+    FrameData data;
+};
+
 class LFR
 {
+public:
+    using ListenerKey = void const*;
+    using ExceptionCallback = std::function<bool(std::exception const &ex)>;
+    using ListenerCallback = std::function<void(LFR_Result)>;
+
+private:
+    using ListenerPair = std::pair<ListenerKey, ListenerCallback>;
+    using ListenerVector = std::vector<ListenerPair>;
+
     Input input;
     Processing processing;
     ControlModule controlModule;
     Interpreter interpreter;
     IntersectionHandler intersectionHandler;
-    volatile bool iAmLooping;
+
-    void loop();
-    thread loopThread;
     int thresholdBinary;
     int gaussKernelSize;
 
-    void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);
+    ListenerVector listeners;
+    ExceptionCallback cb;
+    bool stop;
+    std::unique_ptr<std::thread> thread;
+    mutable std::mutex mutex;
+    
+    //void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);
+    void createThread();
+    void setStop(bool val);
 
 public:
-
     LFR() = delete;
-    LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize);
+    LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, ExceptionCallback cb);
     ~LFR();
 
     void startLoop();
     void endLoop();
-
+    void addListener(ListenerCallback cv, ListenerKey key);
-    
+    void removeListener(ListenerKey key);
-    bool videoFlag;
+    void isStopped() const noexcept;
-    bool saveOutputFlag;
+    Mat provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);    
-
-    std::string outputFileName;
-    
 };