yasarba71520/Line-Following-Robot
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Baran Yasar 2023-01-08 18:29:53 +01:00
commit 1f66795bb1
9 changed files with 245 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
AutonomousMode/ControlModule/control_module.cpp
View File

@ -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]};
}

5
AutonomousMode/ControlModule/control_module.h
View File

@ -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();
};

28
AutonomousMode/Input/input.cpp
View File

@ -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();
}

3
AutonomousMode/Input/input.h
View File

@ -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;

10
AutonomousMode/IntersectionHandler/intersection_handler.cpp
View File

@ -7,3 +7,13 @@ 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;
}

5
AutonomousMode/IntersectionHandler/intersection_handler.h
View File

@ -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);
};

60
AutonomousMode/autonomous_mode_main.cpp
View File

@ -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);
}
}
}

137
AutonomousMode/lfr.cpp
View File

@ -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)
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");}
while(iAmLooping)
{
Mat originalImage = input.readWebcam();
std::lock_guard<std::mutex> lock(mutex);
listeners.emplace_back(key, std::move(cb));
}
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;
void LFR::setStop(bool val)
{
std::lock_guard<std::mutex> lock(mutex);
stop = val;
}
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);
}
if(this->videoFlag) {destroyWindow("Display window");}
input.freeWebcam();
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;
}

47
AutonomousMode/lfr.h
View File

@ -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);
};