From c40b69ebe2bb021ddc06c08e7e2478fc319d2013 Mon Sep 17 00:00:00 2001
From: Tim Zeuner <zeunerti74726@th-nuernberg.de>
Date: Fri, 3 Mar 2023 18:45:03 +0100
Subject: [PATCH] Save video

---
 lfr_state_machine.cpp | 30 ++++++++++++++++++++++--------
 lfr_state_machine.h   |  7 ++++++-
 2 files changed, 28 insertions(+), 9 deletions(-)

diff --git a/lfr_state_machine.cpp b/lfr_state_machine.cpp
index 3952898..15efc78 100644
--- a/lfr_state_machine.cpp
+++ b/lfr_state_machine.cpp
@@ -104,16 +104,26 @@ LFR_StateMachine::LFR_StateMachine():
     currentState = &State::Idle::getInstance();
     currentState->enter(this);
 
-    //Start the permanent loop
-    char input;
-    std::cout << "press q to quit" << std::endl;
-    std::cin >> input;
-    std::cout << "binned" << std::endl;
-    while (input != 'q')
+    cv::VideoWriter writer = cv::VideoWriter("video_200.avi", cv::VideoWriter::fourcc('M','J','P','G'), 4.0, cv::Size(videoWidth, videoHeight), true);
+    
+    auto t_start = std::chrono::high_resolution_clock::now();
+    auto t_end = std::chrono::high_resolution_clock::now();
+    double dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+    
+    while(dur < 60000)
     {
-        std::cin >> input;
-        std::cout << "binned" << std::endl;
+        t_end = std::chrono::high_resolution_clock::now();
+        dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+        {
+            this_thread::sleep_for(std::chrono::milliseconds(50));
+            std::unique_lock<std::mutex> lock(imgMutex);
+            if(!this->image.empty())
+            {
+                writer.write(this->image);
+            }
+        }
     }
+    writer.release();
     std::cout << "Exiting central" << std::endl;
 }
 
@@ -146,6 +156,10 @@ void LFR_StateMachine::enterAutonomous()
             double delta = static_cast<double>(deltaMs) / 1000.0;
             double frameRate = 1.0 / static_cast<double>(delta);*/
             double frameRate = -1.0;
+            {
+                std::unique_lock<std::mutex> lock(imgMutex);
+                image = result.rawImage;
+            }
 
             if (result.validLane)
             {
diff --git a/lfr_state_machine.h b/lfr_state_machine.h
index 2dcb262..7844f5c 100644
--- a/lfr_state_machine.h
+++ b/lfr_state_machine.h
@@ -16,7 +16,7 @@ class LFR_StateMachine
     const int videoHeight = 720;
     const int videoWidth = 1280;
     const int gaussKernelSize = 11;
-    const double maxSpeed = 0.5;
+    const double maxSpeed = 0.20;
 
     std::mutex mutex;
 
@@ -24,6 +24,9 @@ class LFR_StateMachine
     LFR_UART uartCommunicator;
     LFR_Socket socket;
 
+    std::mutex imgMutex;
+    Mat image;
+
     vector<string> split (string s, string delimiter) const;
     void sanitize (string& s) const;
     bool checkStringValidity (const std::vector<std::string>& s) const;
@@ -36,3 +39,5 @@ public:
     inline LFR_IState* getCurrentState() const {return currentState;}
     void setState(LFR_IState& newState);
 };
+
+