Start Autonomous via socket; redirect signals to uart

lfr_state_machine.cpp

@@ -124,4 +124,48 @@ void LFR_StateMachine::setState(LFR_IState& newState)
         std::cout << "no switch" << std::endl;
     }
 
+}
+
+void LFR_StateMachine::enterAutonomous()
+{
+    //cv::Mat img;
+    autonomousMode.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/adapt to 3.5in screen
+            cv::resize(result.rawImage, img, cv::Size(480, 320));
+
+            //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);*/
+            double frameRate = -1.0;
+
+            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;
+                uartCommunicator.sendTelegram(result.motorSignals[0], result.motorSignals[1], result.motorSignals[2], result.motorSignals[3]);
+            }
+            else
+            {
+                std::cout << "No lane found." << std::endl;;
+            }
+            //last = now;
+        }
+    }, &mutex);
+
+    autonomousMode.startLoop();
+}
+
+void LFR_StateMachine::exitAutonomous()
+{
+    autonomousMode.removeListener(&mutex);
+    autonomousMode.endLoop();
 }

lfr_state_machine.h

@@ -29,6 +29,8 @@ class LFR_StateMachine
     void parseString(string s);    
 
 public:
+    void enterAutonomous();
+    void exitAutonomous();
     LFR_StateMachine();
     inline LFR_IState* getCurrentState() const {return currentState;}
     void setState(LFR_IState& newState);

lfr_states.cpp

@@ -19,11 +19,13 @@ LFR_IState& State::Idle::getInstance()
 void State::Autonomous::enter(LFR_StateMachine* stateMachine)
 {
     std::cout << "enter autonomous mode" << std::endl;
+    stateMachine->enterAutonomous();
 }
 
 void State::Autonomous::exit(LFR_StateMachine* stateMachine)
 {
     std::cout << "exit autonomous mode" << std::endl;
+    stateMachine->exitAutonomous();
 }
 
 LFR_IState& State::Autonomous::getInstance()