Line-Following-Robot/AutonomousMode/lfr.cpp

#include "lfr.h"

#define right false
#define left true

LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, double maxSpeed, ExceptionCallback cb):
    stop(false),
    input(videoHeight, videoWidth),
    processing(),
    controlModule(maxSpeed, 1.0, 1.0),
    interpreter(),
    intersectionHandler(),
    cb(cb)
{
    this->thresholdBinary = thresholdBinary;
    this->gaussKernelSize = gaussKernelSize;
}

LFR::~LFR()
{
    endLoop();
    if(thread)
    {
        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))
    {
        listeners.erase(it);
    }
}

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()
{
    if(thread)
    {
        //Restart thread if it is running
        setStop(true);
        thread->join();
        setStop(false);
    }
    createThread();
}

void LFR::endLoop()
{
    setStop(true);
}

cv::Mat LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi)
{
    for(int i = 0; i < frameData.contours.size(); i++)
    {
        drawContours(originalImage, frameData.contours, i, Scalar(0,255,255), 1, 8, noArray(), 0, Point(roi.x, roi.y));

        rectangle(originalImage, frameData.boundingBoxes[i], Scalar(0,255,0));
        Rect center(Point(frameData.middlePoints[i].x-2, frameData.middlePoints[i].y-2), Point(frameData.middlePoints[i].x+2, frameData.middlePoints[i].y+2));
        rectangle(originalImage, center, Scalar(0,0,255));
        Rect leftRect(Point(frameData.leftEdges[i].x-2, frameData.leftEdges[i].y-2), Point(frameData.leftEdges[i].x+2, frameData.leftEdges[i].y+2));
        rectangle(originalImage, leftRect, Scalar(0,0,255));
    }

    if(frameData.contours.size() > 0)
    {
        //Draw the Arrow for the check of the angle
        int length = 100;

        Point contourCenter = frameData.middlePoints[frameData.index];
        Point imageCenter = Point(originalImage.size().width/2.0, originalImage.size().height/2.0);
        Point focalPoint = Point(imageCenter.x, imageCenter.y+1000.0);

        cv::arrowedLine(originalImage, focalPoint, contourCenter, Scalar(0,0,255), 2, 8, 0, 0.03);
        cv::arrowedLine(originalImage, focalPoint, imageCenter, Scalar(0,0,255), 2, 8, 0, 0.03);
    }
    return originalImage;
}