Merge remote-tracking branch 'origin/master'

AutonomousMode/ControlModule/control_module.cpp

@@ -1,10 +1,15 @@
 #include "control_module.h"
 
-ControlModule::ControlModule(){    
+ControlModule::ControlModule(): ControlModule(1.0, 1.0, 1.0)
+{    
 }
 
 ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed)
 {
+    motors[0] = 0.0;
+    motors[1] = 0.0;
+    motors[2] = 0.0;
+    motors[3] = 0.0;
     this->forwardSpeed = forwardSpeed;
     this->rotateSpeed = rotateSpeed;
     this->moveSideSpeed = moveSideSpeed;
@@ -15,13 +20,13 @@ ControlModule::~ControlModule()
 }
 
 void ControlModule::moveForward(){
-    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
+    for(int i = 0; i < 4; i++){
         motors[i] += forwardSpeed;
     }
 };
 
 void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
-    float speed = moveSideSpeed * (contourColumsMiddle - imageColumsMiddle)/imageColumsMiddle;
+    float speed = moveSideSpeed * static_cast<float>(contourColumsMiddle - imageColumsMiddle)/static_cast<float>(imageColumsMiddle);
     motors[0] += speed;
     motors[1] -= speed;
     motors[2] -= speed;
@@ -29,7 +34,7 @@ void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
 }
 
 void ControlModule::rotate(int angle){
-    float speed = rotateSpeed * (angle + 90)/90;
+    float speed = rotateSpeed * (static_cast<float>(angle) + 90.0f)/90.0f;
     motors[0] += speed;
     motors[1] -= speed;
     motors[2] += speed;
@@ -37,19 +42,32 @@ void ControlModule::rotate(int angle){
 }
 
 void ControlModule::unit(){
-    float max = 10E-12;
+    float max = 1.0E-6;
     for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
         if(motors[i] > max)
         max = motors[i];
     }
-    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
-        motors[i] /= max;
+
+    //Avoid dividing by zero
+    if (max > 0.001)
+    {
+        for(int i = 0; i < 4; i++){
+            motors[i] /= max;
+        }
     }
+
 }
 
 void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){
+    std::unique_lock<std::mutex> lock(mtx);
     moveForward();
     moveSide(imageColumsMiddle, contourColumsMiddle);
     rotate(angle);
     unit();
-}
+}
+
+std::vector<float> ControlModule::readMotors()
+{
+    std::unique_lock<std::mutex> lock(mtx);
+    return std::vector<float> {motors[0], motors[1], motors[2], motors[3]};
+} 

AutonomousMode/ControlModule/control_module.h

@@ -1,8 +1,12 @@
 #pragma once
+#include <mutex>
+#include <vector>
+#include <iostream>
 
 class ControlModule
 {
 private:
+    mutable std::mutex mtx;
     float motors[4]; //LeftFront; RightFront; LeftBack; RightBack
     float forwardSpeed;
     float rotateSpeed;
@@ -17,4 +21,5 @@ public:
     void unit();            //Brings the max Value to 1.0
 
     void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle); //Funktion to be called
+    std::vector<float> readMotors();
 };

AutonomousMode/Input/input.cpp

@@ -1,9 +1,10 @@
 #include "input.h"
 
 // TODO: Wenn ihr in die Zeile den Pfad zum Testvideo statt der 0 packt, benmutzt er das Testvideo.
-Input::Input(int videoHeight, int videoWidth) : cap("C:\\Line-Following-Robot\\AutonomousMode\\Test_data\\video1.h264"), videoHeight(videoHeight), videoWidth(videoWidth)
+Input::Input(int videoHeight, int videoWidth) : cap("C:\\Line-Following-Robot\\AutonomousMode\\Test_data\\video1.h264"), videoHeight(videoHeight), videoWidth(videoWidth)//Input::Input(int videoHeight, int videoWidth) : cap(0), videoHeight(videoHeight), videoWidth(videoWidth)
 //Input::Input(int videoHeight, int videoWidth) : cap(0), videoHeight(videoHeight), videoWidth(videoWidth)
 {
+    std::unique_lock<std::mutex> lock(mtx);
     this->cap.set(CAP_PROP_FRAME_HEIGHT, videoHeight);
     this->cap.set(CAP_PROP_FRAME_WIDTH, videoWidth);
 }
@@ -15,22 +16,25 @@ Input::~Input()
 
 Mat Input::readFile(String filePath)
 {
-    std::srand(std::time(0));
+    std::unique_lock<std::mutex> lock(mtx);
+    std::srand(static_cast<unsigned int>(std::time(0)));
     // Read all .jpg files from the specified folder
     cv::String folder = filePath;
     std::vector<cv::String> filenames;
     cv::glob(folder, filenames);
 
     // Random shuffle
-    std::random_shuffle(filenames.begin(), filenames.end());
+    std::random_device rd;
+    std::mt19937 g(rd());
+    std::shuffle(filenames.begin(), filenames.end(), g);
 
     Mat image = imread(filePath, IMREAD_COLOR);
     
     if(image.empty())
     {
-        std::cout << "Could not read the image: " << filePath << std::endl;
-        return Mat();
-        //To do:Exception handeling
+        stringstream sstream;
+        sstream << "Could not read the image: " << filePath << std::endl;
+        throw std::runtime_error(sstream.str());
     }
     resize(image, image, Size(this->videoWidth, this->videoHeight));
     return image;
@@ -38,16 +42,19 @@ Mat Input::readFile(String filePath)
 
 Mat Input::readWebcam()
 {
+    std::unique_lock<std::mutex> lock(mtx);
     Mat image;
 
     if(!cap.isOpened()) {
-        cout << "Video capture not opened" << std::endl;
-        return Mat();
+        stringstream sstream;
+        sstream << "Video capture not opened" << std::endl;
+        throw std::runtime_error(sstream.str());
     }
 
     if(!cap.grab()) {
-        cout << "Could not grab frame from camera" << std::endl;
-        return Mat();
+        stringstream sstream;
+        sstream << "Could not grab frame from camera" << std::endl;
+        throw std::runtime_error(sstream.str());
     }   
     cap.retrieve(image);
     return image;  
@@ -55,5 +62,6 @@ Mat Input::readWebcam()
 
 void Input::freeWebcam()
 {
+    std::unique_lock<std::mutex> lock(mtx);
     this->cap.release();
 }

AutonomousMode/Input/input.h

@@ -4,6 +4,7 @@
 #include <vector>
 #include <string>
 #include <algorithm>
+#include <random>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/core/utils/logger.hpp>
@@ -15,7 +16,7 @@ class Input
 {
 private:
     VideoCapture cap;
-
+    mutable std::mutex mtx;
 public:
     int videoHeight;
     int videoWidth;

AutonomousMode/IntersectionHandler/intersection_handler.cpp

@@ -6,4 +6,14 @@ IntersectionHandler::IntersectionHandler(/* args */)
 
 IntersectionHandler::~IntersectionHandler()
 {
+}
+
+const bool IntersectionHandler::foundLane(const FrameData& data)
+{
+    return data.boundingBoxes.size();
+}
+
+const bool IntersectionHandler::foundIntersection(const FrameData& data)
+{
+    return data.boundingBoxes.size()>1;
 }

AutonomousMode/IntersectionHandler/intersection_handler.h

@@ -1,11 +1,14 @@
 #pragma once
+#include <utils.h>
 
 class IntersectionHandler
 {
 private:
-    /* data */
 public:
     IntersectionHandler(/* args */);
 
     ~IntersectionHandler();
+
+    const bool foundLane(const FrameData& data);
+    const bool foundIntersection(const FrameData& data);
 };

AutonomousMode/autonomous_mode_main.cpp

@@ -4,19 +4,63 @@
 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);
-    lfr.saveOutputFlag = false;
-    lfr.videoFlag = true;
+    std::mutex mutex;
+
+    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 false;
+    });
+
+    //To calculate the frame rate
+    std::chrono::milliseconds last = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+    std::chrono::milliseconds now = std::chrono::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())
+        {
+            img = result.rawImage;
+            //Resize to minimize latency using raspi over ssh
+            //cv::resize(result.rawImage, img, cv::Size(128, 64+32));
+
+            //Calculate frame rate
+            now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+            unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
+            double delta = static_cast<double>(deltaMs) / 1000.0;
+            double frameRate = 1.0 / static_cast<double>(delta);
+
+            if (result.validLane)
+            {
+                std::cout << "Frame rate: " << frameRate << " angle: " << result.data.angle
+                    << " motor 1: " << result.motorSignals[0] << " motor 2: " << result.motorSignals[1]
+                    << " motor 3: " << result.motorSignals[2] << " motor 4: " << result.motorSignals[3] <<std::endl;
+            }
+            else
+            {
+                std::cout << "No lane found." << std::endl;;
+            }
+            last = now;
+        }
+    }, &mutex);
+
     lfr.startLoop();
-    //To end the video stream, write any char in the console.
-    char a;
-    std::cin >> a; 
-    lfr.endLoop();
+    
+    for(int finished = false; finished != 'q';){
+        finished = std::tolower(cv::waitKey(1));
+        std::unique_lock<std::mutex> lock(mutex);
+        if(!img.empty()){
+            cv::imshow("frame", img);
+        }
+    }
 }

AutonomousMode/lfr.cpp

@@ -3,63 +3,123 @@
 #define right false
 #define left true
 
-LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize)
-    : iAmLooping(false), input(videoHeight, videoWidth), processing(), controlModule(), interpreter(), intersectionHandler()
+LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, ExceptionCallback cb):
+    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)
-    {
-        this->endLoop();
-    }
+    endLoop();
+    thread->join();
 }
 
-void LFR::loop()
+void LFR::removeListener(LFR::ListenerKey key)
 {
-    if(this->videoFlag) {namedWindow("Display window");}
-    while(iAmLooping)
+    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))
     {
-        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(roi);
-        Mat processedImage = originalImage;
-
-        processing.processImage(processedImage, this->thresholdBinary, this->gaussKernelSize);
-        //processedImage = processedImage(roi);
-        FrameData data = processing.calculateLineSegments(processedImage, roi);
-        processing.filterReflections(data);
-        processing.calcAngles(data, originalImage.cols, originalImage.rows, left);
-        this->provideOutput(originalImage, processedImage, data, roi);
+        listeners.erase(it);
     }
-    if(this->videoFlag) {destroyWindow("Display window");}
-    input.freeWebcam();
+}
+
+void LFR::addListener(LFR::ListenerCallback cb, LFR::ListenerKey key)
+{
+    std::lock_guard<std::mutex> lock(mutex);
+    listeners.emplace_back(key, std::move(cb));
+}
+
+void LFR::setStop(bool val)
+{
+    std::lock_guard<std::mutex> lock(mutex);
+    stop = val;
+}
+
+void LFR::createThread()
+{
+    thread = std::make_unique<std::thread>([this](){
+        while(true)
+        {
+            LFR_Result result;
+            if(!stop && !listeners.empty())
+            {
+                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);
+                    
+                    result.validLane = intersectionHandler.foundLane(data);
+                    if (result.validLane)
+                    {
+                        controlModule.calcSpeeds(1,1, data.angle);
+                        processedImage = provideOutput(originalImage, processedImage, data, roi);
+                    }
+                    result.rawImage = originalImage;
+                    result.processedImage = processedImage;
+                    result.data = data;
+                    result.motorSignals = controlModule.readMotors();
+                }
+                catch(std::exception const &ex)
+                {
+                    if(!cb(ex))
+                    {
+                        //callback returned false -> exception not handled -> exit
+                        exit(EXIT_FAILURE);
+                    }
+                }
+
+                //Invoke the callback method (ListenerPair second -> ListenerCallback)
+                for(auto &val : listeners)
+                {
+                    val.second(result);
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+    });
 }
 
 void LFR::startLoop()
 {
-    iAmLooping = true;
-    this->loopThread=thread(&LFR::loop, this);
+    if(thread)
+    {
+        //Restart thread if it is running
+        setStop(true);
+        thread->join();
+        setStop(false);
+    }
+    createThread();
 }
 
 void LFR::endLoop()
 {
-    iAmLooping = false;
-    this->loopThread.join();
-    return;
+    setStop(true);
 }
 
-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 +143,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)
-    {
-        imshow("Display window", originalImage);
-        imshow("processed:", processedImage);
-        char c = (char)waitKey(25);
-    }
-    if (this->saveOutputFlag && !(this->outputFileName.empty()))
-    {
-        imwrite(this->outputFileName, originalImage);        
-    }
+    return originalImage;
 }

AutonomousMode/lfr.h

@@ -3,6 +3,7 @@
 #include <iostream>
 #include <future>
 #include <thread>
+#include <functional>
 
 #include <opencv2/opencv.hpp>
 
@@ -15,34 +16,54 @@
 
 using namespace cv;
 
+struct LFR_Result
+{
+    bool validLane;
+    cv::Mat rawImage;
+    cv::Mat processedImage;
+    FrameData data;
+    std::vector<float> motorSignals;
+};
+
 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();
-
-    
-    bool videoFlag;
-    bool saveOutputFlag;
-
-    std::string outputFileName;
-    
+    void addListener(ListenerCallback cv, ListenerKey key);
+    void removeListener(ListenerKey key);
+    void isStopped() const noexcept;
+    Mat provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);    
 };