Merge branch 'master' of https://git.efi.th-nuernberg.de/gitea/yasarba71520/Line-Following-Robot

Communication/uart_communication.cpp

@@ -14,7 +14,7 @@ void LFR_UART::openFile(const char *fileName) {
 }
 
 int LFR_UART::writeDataToFile(uint8_t *buff, uint32_t bufferLength) {
-    std::cout << "Sending uart telegram" << std::endl;
+    //std::cout << "Sending uart telegram" << std::endl;
     return write(this->fileDescriptor, buff, bufferLength);
 }
 

Socket/lfr_socket.cpp

@@ -2,6 +2,7 @@
 
 LFR_Socket::LFR_Socket(ExceptionCallback cb): cb(cb)
 {
+    bufferIterator = std::begin(buffer);
     if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
     {
         throw std::runtime_error("Failed opening the socket");
@@ -66,7 +67,7 @@ void LFR_Socket::createThread()
         bool connected = false;
         while(true)
         {
-            LFR_Socket::LFR_Telegram telegram;
+            LFR_Socket::LFR_Telegram telegram = {};
             bool received = false;
             if(!stop && !listeners.empty())
             {
@@ -91,32 +92,46 @@ void LFR_Socket::createThread()
                                 }
                                 else
                                 {
-                                    //std::cout << "accepted connection" << std::endl;
+                                    std::cout << "accepted connection" << std::endl;
                                     pollfds[1].fd = new_socket;
                                     connected  = true;
                                 }
                             }
                             else if (connected  && pollfds[0].revents & POLLIN)
                             {
-                                //std::cout << "second connection incoming" << std::endl;
+                                std::cout << "second connection incoming" << std::endl;
                                 int tmp_socket = accept(server_fd, (struct sockaddr *)&address, (socklen_t *)&addrlen);
                                 close(tmp_socket);
-                                //std::cout << "second connection closed" << std::endl;
+                                std::cout << "second connection closed" << std::endl;
                             }
                             else if (pollfds[1].revents & POLLIN)
                             {
-                                bytes_received = recv(new_socket, buffer, sizeof(buffer), 0);
-                                if (bytes_received > 0)
+                                LFR_Telegram tmpBuffer = {0};
+                                bytesReceived = recv(new_socket, tmpBuffer, sizeof(tmpBuffer), 0);
+                                if (bytesReceived > 0)
                                 {
-                                    received = true;
-                                    std::copy(std::begin(buffer), std::end(buffer), std::begin(telegram));
-                                    //std::cout << "message received" << std::endl;
+                                    if(bufferIterator - std::begin(buffer) + bytesReceived > 1024)
+                                    {
+                                        //Too long for buffer. Clear it.
+                                        std::fill(std::begin(buffer), std::end(buffer), 0);
+                                        bufferIterator = std::begin(buffer);
+                                    }
+                                    bufferIterator = std::copy_if(
+                                        std::begin(tmpBuffer),
+                                        std::begin(tmpBuffer)+bytesReceived, 
+                                        bufferIterator,
+                                        [](char c){
+                                            return(c != '\n' && c != '\r');
+                                        });
+
+                                    //std::cout << "bytes received: " << bytesReceived << std::endl;
+                                    //received = true;
                                 }
-                                else if(bytes_received == 0)
+                                else if(bytesReceived == 0)
                                 {
                                     connected = false;
                                     close(pollfds[1].fd);
-                                    //std::cout << "connection closed by peer" << std::endl;
+                                    std::cout << "connection closed by peer" << std::endl;
                                 }
                             }
                         }
@@ -131,6 +146,32 @@ void LFR_Socket::createThread()
                     }
                 }
 
+                // Search the buffer for the start of a telegram
+                char *telegramStart = NULL;
+                telegramStart = strstr(buffer, "aa");
+                if(telegramStart && telegramStart != std::begin(buffer)) {
+                    //Move the content of the buffer
+                    auto delta = bufferIterator - telegramStart;
+                    std::copy(telegramStart, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer) + delta;
+                }
+
+                // Search the buffer for the end of a telegram
+                char *telegramEnd = NULL;
+                telegramEnd = strstr(buffer, "zz");
+                if(telegramEnd && telegramStart && telegramEnd-telegramStart>2) {
+                    std::copy(telegramStart+2, telegramEnd, telegram);
+                    received = true;
+                    std::copy(telegramEnd+2, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer);
+                }
+                else if(telegramEnd && telegramStart && telegramEnd-telegramStart<=2)
+                {
+                    //Got an empty telegram
+                    std::copy(telegramEnd, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer);
+                }
+                
                 //Invoke the callback method (ListenerPair second -> ListenerCallback)
                 if (received)
                 {

Socket/lfr_socket.h

@@ -13,6 +13,7 @@
 #include <string>
 #include <algorithm>
 #include <exception>
+#include <cstring>
 
 class LFR_Socket
 {
@@ -31,11 +32,12 @@ private:
     std::unique_ptr<std::thread> thread;
     mutable std::mutex mutex;
 
-    int server_fd, new_socket, bytes_received;
+    int server_fd, new_socket, bytesReceived;
     struct sockaddr_in address;
     int opt = 1;
     int addrlen = sizeof(address);
     char buffer[1024] = {0};    
+    char* bufferIterator;
 
     
     //void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);

lfr_state_machine.cpp

@@ -57,9 +57,13 @@ void LFR_StateMachine::parseString(string s)
     std::vector<std::string> splitStr = split(s, ";");
     if(!checkStringValidity(splitStr))
     {
-        std::cout<<"Invalid String" << std::endl;
+        std::cout<<"Error: Invalid String: " << s << std::endl;
         return;
     }
+    else
+    {
+        std::cout<< s << std::endl;
+    }
     double wheels[4] = {0.0, 0.0, 0.0, 0.0};
     int mode = std::stoi(splitStr[0]);
     if(mode == 0) {
@@ -121,11 +125,6 @@ void LFR_StateMachine::setState(LFR_IState& newState)
         currentState = &newState;
         currentState->enter(this);
     }
-    else
-    {
-        std::cout << "no switch" << std::endl;
-    }
-
 }
 
 void LFR_StateMachine::enterAutonomous()