From b46898a506c2606df53fce125fdee032714b7053 Mon Sep 17 00:00:00 2001 From: Tim Zeuner Date: Thu, 22 Dec 2022 23:12:21 +0100 Subject: [PATCH 1/9] cmake standard to cpp 20 --- AutonomousMode/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/AutonomousMode/CMakeLists.txt b/AutonomousMode/CMakeLists.txt index 8ca822e..5d79b6d 100644 --- a/AutonomousMode/CMakeLists.txt +++ b/AutonomousMode/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.1.0) project(lfr_image_processing VERSION 0.1.0) +set (CMAKE_CXX_STANDARD 20) + include(CTest) enable_testing() From b5811dbbcb44d7c914e54342c371e48c9cf696d7 Mon Sep 17 00:00:00 2001 From: Tim Zeuner Date: Thu, 22 Dec 2022 23:13:53 +0100 Subject: [PATCH 2/9] Ressource guarding multithreading --- AutonomousMode/autonomous_mode_main.cpp | 49 +++++++-- AutonomousMode/lfr.cpp | 128 ++++++++++++++++-------- AutonomousMode/lfr.h | 45 ++++++--- 3 files changed, 158 insertions(+), 64 deletions(-) diff --git a/AutonomousMode/autonomous_mode_main.cpp b/AutonomousMode/autonomous_mode_main.cpp index 63abfd1..fcbc4f3 100644 --- a/AutonomousMode/autonomous_mode_main.cpp +++ b/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); - lfr.saveOutputFlag = false; - lfr.videoFlag = true; + std::mutex mutex; + + LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex) + { + std::unique_lock lock(mutex); + std::cerr<<"camera exception:"<(std::chrono::system_clock::now().time_since_epoch()); + std::chrono::milliseconds now = duration_cast(std::chrono::system_clock::now().time_since_epoch()); + + cv::Mat img; + lfr.addListener([&](LFR_Result result) + { + std::unique_lock lock(mutex); + if (!result.rawImage.empty()) + { + cv::resize(result.rawImage, img, cv::Size(128, 64+32)); + + //Calculate frame rate + now = duration_cast(std::chrono::system_clock::now().time_since_epoch()); + unsigned int deltaMs = (now-last).count(); + float delta = static_cast(deltaMs) / 1000.0; + float frameRate = 1.0 / static_cast(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; - std::cin >> a; - lfr.endLoop(); + + for(int finished = false; finished != 'q';){ + finished = std::tolower(cv::waitKey(66)); + std::unique_lock lock(mutex); + if(!img.empty()){ + cv::imshow("frame", img); + } + } } diff --git a/AutonomousMode/lfr.cpp b/AutonomousMode/lfr.cpp index 4343ae2..28feaa6 100644 --- a/AutonomousMode/lfr.cpp +++ b/AutonomousMode/lfr.cpp @@ -3,63 +3,114 @@ #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 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 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 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([this](){ + while(true) + { + LFR_Result result; + if(!stop && !listeners.empty()) + { + try + { + std::lock_guard 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; - 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 +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) - { - imshow("Display window", originalImage); - imshow("processed:", processedImage); - char c = (char)waitKey(25); - } - if (this->saveOutputFlag && !(this->outputFileName.empty())) - { - imwrite(this->outputFileName, originalImage); - } + return originalImage; } diff --git a/AutonomousMode/lfr.h b/AutonomousMode/lfr.h index 22140fb..b421d16 100644 --- a/AutonomousMode/lfr.h +++ b/AutonomousMode/lfr.h @@ -3,6 +3,7 @@ #include #include #include +#include #include @@ -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; + using ListenerCallback = std::function; + +private: + using ListenerPair = std::pair; + using ListenerVector = std::vector; + 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 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); }; \ No newline at end of file From 4c9435dbee400a2f456204e9408538f05569612d Mon Sep 17 00:00:00 2001 From: TimZnr Date: Wed, 4 Jan 2023 16:06:11 +0100 Subject: [PATCH 3/9] nvm the c++ 20 thing --- AutonomousMode/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/AutonomousMode/CMakeLists.txt b/AutonomousMode/CMakeLists.txt index 5d79b6d..8ca822e 100644 --- a/AutonomousMode/CMakeLists.txt +++ b/AutonomousMode/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.1.0) project(lfr_image_processing VERSION 0.1.0) -set (CMAKE_CXX_STANDARD 20) - include(CTest) enable_testing() From 7fe6850da48018b80a8bc347819a1d5f7a7a1bab Mon Sep 17 00:00:00 2001 From: TimZnr Date: Wed, 4 Jan 2023 16:08:38 +0100 Subject: [PATCH 4/9] No idea why this worked on the pi --- AutonomousMode/autonomous_mode_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/AutonomousMode/autonomous_mode_main.cpp b/AutonomousMode/autonomous_mode_main.cpp index fcbc4f3..dbd44a7 100644 --- a/AutonomousMode/autonomous_mode_main.cpp +++ b/AutonomousMode/autonomous_mode_main.cpp @@ -21,8 +21,8 @@ int main(void) }); //To calculate the frame rate - std::chrono::milliseconds last = duration_cast(std::chrono::system_clock::now().time_since_epoch()); - std::chrono::milliseconds now = duration_cast(std::chrono::system_clock::now().time_since_epoch()); + std::chrono::milliseconds last = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); + std::chrono::milliseconds now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); cv::Mat img; lfr.addListener([&](LFR_Result result) @@ -33,12 +33,12 @@ int main(void) cv::resize(result.rawImage, img, cv::Size(128, 64+32)); //Calculate frame rate - now = duration_cast(std::chrono::system_clock::now().time_since_epoch()); - unsigned int deltaMs = (now-last).count(); - float delta = static_cast(deltaMs) / 1000.0; - float frameRate = 1.0 / static_cast(delta); + now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); + unsigned int deltaMs = static_cast((now-last).count()); + double delta = static_cast(deltaMs) / 1000.0; + double frameRate = 1.0 / static_cast(delta); - //std::cout << "Frame rate: " << frameRate << std::endl; + std::cout << "Frame rate: " << frameRate << std::endl; last = now; } }, &mutex); From f55ebb6b3fc1e8f23132fe9c3b4be2617d8244b1 Mon Sep 17 00:00:00 2001 From: TimZnr Date: Wed, 4 Jan 2023 16:52:42 +0100 Subject: [PATCH 5/9] exception handling --- AutonomousMode/Input/input.cpp | 20 ++++++++++++-------- AutonomousMode/Input/input.h | 1 + AutonomousMode/autonomous_mode_main.cpp | 8 +++++--- AutonomousMode/lfr.cpp | 6 +++++- 4 files changed, 23 insertions(+), 12 deletions(-) diff --git a/AutonomousMode/Input/input.cpp b/AutonomousMode/Input/input.cpp index fee41a1..7dd90cf 100644 --- a/AutonomousMode/Input/input.cpp +++ b/AutonomousMode/Input/input.cpp @@ -22,15 +22,17 @@ Mat Input::readFile(String filePath) 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; @@ -41,13 +43,15 @@ Mat Input::readWebcam() 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; diff --git a/AutonomousMode/Input/input.h b/AutonomousMode/Input/input.h index c4accac..c87191a 100644 --- a/AutonomousMode/Input/input.h +++ b/AutonomousMode/Input/input.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include diff --git a/AutonomousMode/autonomous_mode_main.cpp b/AutonomousMode/autonomous_mode_main.cpp index dbd44a7..ca229bc 100644 --- a/AutonomousMode/autonomous_mode_main.cpp +++ b/AutonomousMode/autonomous_mode_main.cpp @@ -17,7 +17,7 @@ int main(void) { std::unique_lock lock(mutex); std::cerr<<"camera exception:"< lock(mutex); if (!result.rawImage.empty()) { - cv::resize(result.rawImage, img, cv::Size(128, 64+32)); + 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::system_clock::now().time_since_epoch()); @@ -46,7 +48,7 @@ int main(void) lfr.startLoop(); for(int finished = false; finished != 'q';){ - finished = std::tolower(cv::waitKey(66)); + finished = std::tolower(cv::waitKey(1)); std::unique_lock lock(mutex); if(!img.empty()){ cv::imshow("frame", img); diff --git a/AutonomousMode/lfr.cpp b/AutonomousMode/lfr.cpp index 28feaa6..d445464 100644 --- a/AutonomousMode/lfr.cpp +++ b/AutonomousMode/lfr.cpp @@ -76,7 +76,11 @@ void LFR::createThread() } catch(std::exception const &ex) { - cb(ex); + if(!cb(ex)) + { + //callback returned false -> exception not handled -> exit + exit(EXIT_FAILURE); + } } //Invoke the callback method (ListenerPair second -> ListenerCallback) From 8150850bb6bef7570b329a36f00185425c1dda1b Mon Sep 17 00:00:00 2001 From: TimZnr Date: Wed, 4 Jan 2023 21:47:54 +0100 Subject: [PATCH 6/9] float mistakes --- .../ControlModule/control_module.cpp | 34 ++++++++++++++----- AutonomousMode/ControlModule/control_module.h | 5 +++ 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp index 2d2ab87..b6666b0 100644 --- a/AutonomousMode/ControlModule/control_module.cpp +++ b/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(contourColumsMiddle - imageColumsMiddle)/static_cast(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(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 lock(mtx); moveForward(); moveSide(imageColumsMiddle, contourColumsMiddle); rotate(angle); unit(); -} \ No newline at end of file +} + +std::vector ControlModule::readMotors() +{ + std::unique_lock lock(mtx); + return std::vector {motors[0], motors[1], motors[2], motors[3]}; +} \ No newline at end of file diff --git a/AutonomousMode/ControlModule/control_module.h b/AutonomousMode/ControlModule/control_module.h index ab99c88..fc7d5b6 100644 --- a/AutonomousMode/ControlModule/control_module.h +++ b/AutonomousMode/ControlModule/control_module.h @@ -1,8 +1,12 @@ #pragma once +#include +#include +#include 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 readMotors(); }; \ No newline at end of file From 747053f1327083eaa20529b5e5d97458ddbf775e Mon Sep 17 00:00:00 2001 From: TimZnr Date: Wed, 4 Jan 2023 21:50:02 +0100 Subject: [PATCH 7/9] input threadsafe --- AutonomousMode/Input/input.cpp | 8 ++++++-- AutonomousMode/Input/input.h | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/AutonomousMode/Input/input.cpp b/AutonomousMode/Input/input.cpp index 7dd90cf..3005c0c 100644 --- a/AutonomousMode/Input/input.cpp +++ b/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 lock(mtx); this->cap.set(CAP_PROP_FRAME_HEIGHT, videoHeight); this->cap.set(CAP_PROP_FRAME_WIDTH, videoWidth); } @@ -15,7 +16,8 @@ Input::~Input() Mat Input::readFile(String filePath) { - std::srand(std::time(0)); + std::unique_lock lock(mtx); + std::srand(static_cast(std::time(0))); // Read all .jpg files from the specified folder cv::String folder = filePath; std::vector filenames; @@ -40,6 +42,7 @@ Mat Input::readFile(String filePath) Mat Input::readWebcam() { + std::unique_lock lock(mtx); Mat image; if(!cap.isOpened()) { @@ -59,5 +62,6 @@ Mat Input::readWebcam() void Input::freeWebcam() { + std::unique_lock lock(mtx); this->cap.release(); } diff --git a/AutonomousMode/Input/input.h b/AutonomousMode/Input/input.h index c87191a..32b467a 100644 --- a/AutonomousMode/Input/input.h +++ b/AutonomousMode/Input/input.h @@ -16,7 +16,7 @@ class Input { private: VideoCapture cap; - + mutable std::mutex mtx; public: int videoHeight; int videoWidth; From 7175b68abec1c87651a5c4200e37237cca189fb4 Mon Sep 17 00:00:00 2001 From: TimZnr Date: Wed, 4 Jan 2023 21:50:59 +0100 Subject: [PATCH 8/9] motor signals in the loop --- AutonomousMode/autonomous_mode_main.cpp | 4 +++- AutonomousMode/lfr.cpp | 3 +++ AutonomousMode/lfr.h | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/AutonomousMode/autonomous_mode_main.cpp b/AutonomousMode/autonomous_mode_main.cpp index ca229bc..7976423 100644 --- a/AutonomousMode/autonomous_mode_main.cpp +++ b/AutonomousMode/autonomous_mode_main.cpp @@ -40,7 +40,9 @@ int main(void) double delta = static_cast(deltaMs) / 1000.0; double frameRate = 1.0 / static_cast(delta); - std::cout << "Frame rate: " << frameRate << std::endl; + 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] < motorSignals; }; class LFR From ff7634e0b084aed28a6cfade7be5d55c2af61233 Mon Sep 17 00:00:00 2001 From: TimZnr Date: Thu, 5 Jan 2023 10:39:48 +0100 Subject: [PATCH 9/9] Check for valid lane --- .../IntersectionHandler/intersection_handler.cpp | 10 ++++++++++ .../IntersectionHandler/intersection_handler.h | 5 ++++- AutonomousMode/autonomous_mode_main.cpp | 13 ++++++++++--- AutonomousMode/lfr.cpp | 10 ++++++---- AutonomousMode/lfr.h | 1 + 5 files changed, 31 insertions(+), 8 deletions(-) diff --git a/AutonomousMode/IntersectionHandler/intersection_handler.cpp b/AutonomousMode/IntersectionHandler/intersection_handler.cpp index a6b66e0..1786040 100644 --- a/AutonomousMode/IntersectionHandler/intersection_handler.cpp +++ b/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; } \ No newline at end of file diff --git a/AutonomousMode/IntersectionHandler/intersection_handler.h b/AutonomousMode/IntersectionHandler/intersection_handler.h index 2f18184..9080418 100644 --- a/AutonomousMode/IntersectionHandler/intersection_handler.h +++ b/AutonomousMode/IntersectionHandler/intersection_handler.h @@ -1,11 +1,14 @@ #pragma once +#include class IntersectionHandler { private: - /* data */ public: IntersectionHandler(/* args */); ~IntersectionHandler(); + + const bool foundLane(const FrameData& data); + const bool foundIntersection(const FrameData& data); }; \ No newline at end of file diff --git a/AutonomousMode/autonomous_mode_main.cpp b/AutonomousMode/autonomous_mode_main.cpp index 7976423..a3f4414 100644 --- a/AutonomousMode/autonomous_mode_main.cpp +++ b/AutonomousMode/autonomous_mode_main.cpp @@ -40,9 +40,16 @@ int main(void) double delta = static_cast(deltaMs) / 1000.0; double frameRate = 1.0 / static_cast(delta); - 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] <