UDP a peu pres OK

software/monitor/monitor/Client.cs

@@ -164,6 +164,11 @@ namespace monitor
                     Console.WriteLine("Connection to server dropped: " + e.ToString());
                     return;
                 }
+                catch (System.IO.IOException e)
+                {
+                    readEvent?.Invoke(null, null);
+                    return ;
+                }
 
                 newLength = 1;
 

software/monitor/monitor/ClientUDP.cs

@@ -8,62 +8,47 @@ namespace monitor
     public static class ClientUDP
     {
         private const int listenPort = 11000;
-        private static byte[] ImageBuffer = null;
 
-        private static UdpClient listener = null;
+        private static UdpClient clientUDP = null;
-        private static IPEndPoint groupEP = null;
+        private static IPEndPoint ep = null;
 
-        public static void UDPOpen(int port)
+        public static void UDPOpen(string addr, int port)
         {
-            listener = new UdpClient(port);
+            clientUDP = new UdpClient(port);
-            groupEP = new IPEndPoint(IPAddress.Any, port);
+            ep = new IPEndPoint(IPAddress.Parse(addr), port);
+            clientUDP.Connect(ep);
+
+            SendPing();
         }
 
         public static void UDPClose()
         {
-            listener.Close();
+            if (clientUDP!=null)
+                clientUDP.Close();
         }
 
-        public static byte[] GetImage()
+        public static byte[] GetData()
         {
-            bool done = false;
+            Console.WriteLine("Waiting for broadcast");
+            byte[] bytes = clientUDP.Receive(ref ep);
 
-            try
+            Console.WriteLine("Received broadcast from {0} :\n {1}\n",
-            {
+                ep.ToString(),
-                while (!done)
+                Encoding.ASCII.GetString(bytes, 0, bytes.Length));
-                {
-                    Console.WriteLine("Waiting for broadcast");
-                    byte[] bytes = listener.Receive(ref groupEP);
             
-                    Console.WriteLine("Received broadcast from {0} :\n {1}\n",
+            return bytes;
-                        groupEP.ToString(),
+        }
-                        Encoding.ASCII.GetString(bytes, 0, bytes.Length));
 
-                    if (bytes[0]=='I') {
+        public static bool SendPing()
-                        // Nouvelle trame recu
+        {
-                        ImageBuffer = bytes;
+            byte[] msg = new byte[2];
-                    }
+            msg[0] = (byte)'O';
-                    else if (bytes[bytes.Length-1]=='D')
+            msg[1] = (byte)'k';
-                    {
-                        Array.Resize<byte>(ref ImageBuffer, ImageBuffer.Length + bytes.Length); // resize currrent buffer
 
-                        System.Buffer.BlockCopy(ImageBuffer, 0, bytes, ImageBuffer.Length-bytes.Length, bytes.Length);
+            Console.WriteLine("Ping Server to send address");
-                        done = true;
+            clientUDP.Send(msg, msg.Length);
-                    }
-                    else{
-                        Array.Resize<byte>(ref ImageBuffer, ImageBuffer.Length + bytes.Length); // resize currrent buffer
 
-                        System.Buffer.BlockCopy(ImageBuffer, 0, bytes, ImageBuffer.Length-bytes.Length, bytes.Length);
+            return true;
-                    }
-                }
-
-            }
-            catch (Exception e)
-            {
-                Console.WriteLine(e.ToString());
-            }
-     
-            return ImageBuffer;
         }
     }
 }

software/monitor/monitor/MonitorUI.cs

@@ -24,6 +24,8 @@ using System;
 using Gtk;
 using Gdk;
 
+using System.Threading;
+
 using monitor;
 
 /// <summary>
@@ -61,9 +63,28 @@ public partial class MainWindow : Gtk.Window
     /// </summary>
     private System.Timers.Timer batteryTimer;
 
+    /// <summary>
+    /// Counter for total image received, reset after CAM_OPEN is sent
+    /// </summary>
     private int imageReceivedCounter = 0;
+
+    /// <summary>
+    /// Counter increased each time incorrect image is received
+    /// </summary>
     private int badImageReceivedCounter = 0;
 
+    /// <summary>
+    /// Buffer containing a complete image.
+    /// </summary>
+    private byte[] imageComplete;
+
+    /// <summary>
+    /// Buffer used to store incoming image.
+    /// </summary>
+    private byte[] imageInProgress;
+
+    private Thread imageThread;
+
     /// <summary>
     /// Initializes a new instance of the <see cref="MainWindow"/> class.
     /// </summary>
@@ -120,7 +141,11 @@ public partial class MainWindow : Gtk.Window
 
                 checkButtonCameraOn.Active = false;
                 checkButtonRobotPosition.Active = false;
-                if (cmdManager != null) cmdManager.Close();
+                if (cmdManager != null)
+                {
+                    cmdManager.Close();
+                    ClientUDP.UDPClose();
+                }
 
                 batteryTimer.Stop();
                 break;
@@ -195,14 +220,16 @@ public partial class MainWindow : Gtk.Window
     {
         Console.WriteLine("Bye bye");
 
-        if (cmdManager != null) cmdManager.Close();
+        if (cmdManager != null)
+        {
+            cmdManager.Close();
+            ClientUDP.UDPClose();
+        }
+
         Application.Quit();
         a.RetVal = true;
     }
 
-    private byte[] imageComplete;
-    private byte[] imageInProgress;
-
     /// <summary>
     /// Callback called when new message is received from server
     /// </summary>
@@ -211,7 +238,7 @@ public partial class MainWindow : Gtk.Window
     /// <param name="buffer">Raw buffer corresponding of received message</param>
     public void OnCommandReceivedEvent(string header, string data, byte[] buffer)
     {
-        if (buffer==null)
+        if (buffer == null)
         {
             // we have lost server
             ChangeState(SystemState.NotConnected);
@@ -220,6 +247,7 @@ public partial class MainWindow : Gtk.Window
                      ButtonsType.Ok, "Server lost",
                          "Server is down: disconnecting");
             cmdManager.Close();
+            ClientUDP.UDPClose();
         }
 
         // if we have received a valid message
@@ -231,10 +259,6 @@ public partial class MainWindow : Gtk.Window
                 Console.WriteLine("Bad header(" + buffer.Length + ")");
             else
                 Console.WriteLine("Received header (" + header.Length + "): " + header);
-            //if (header.ToUpper() != DestijlCommandList.HeaderStmImage)
-            //{
-            //    if (data != null) Console.WriteLine("Received data (" + data.Length + "): " + data);
-            //}
 #endif
             // Image management
             if (header == DestijlCommandList.HeaderStmImage)
@@ -272,32 +296,6 @@ public partial class MainWindow : Gtk.Window
                         break;
                 }
             }
-            else if (header.ToUpper() == DestijlCommandList.HeaderStmImage)
-            {
-                // if message is an image, convert it to a pixbuf
-                // that can be displayed
-                if (imageComplete != null)
-                {
-                    byte[] image = new byte[imageComplete.Length - 4];
-                    System.Buffer.BlockCopy(imageComplete, 4, image, 0, image.Length);
-
-                    imageReceivedCounter++;
-                    try
-                    {
-                        drawingareaCameraPixbuf = new Pixbuf(image);
-                        drawingAreaCamera.QueueDraw();
-                    }
-                    catch (GLib.GException)
-                    {
-                        badImageReceivedCounter++;
-#if DEBUG
-                        Console.WriteLine("Bad Image: " + badImageReceivedCounter +
-                                          " / " + imageReceivedCounter +
-                                          " (" + badImageReceivedCounter * 100 / imageReceivedCounter + "%)");
-#endif
-                    }
-                }
-            }
         }
     }
 
@@ -309,7 +307,12 @@ public partial class MainWindow : Gtk.Window
     protected void OnQuitActionActivated(object sender, EventArgs e)
     {
         Console.WriteLine("Bye bye 2");
-        if (cmdManager != null) cmdManager.Close();
+        if (cmdManager != null)
+        {
+            cmdManager.Close();
+            ClientUDP.UDPClose();
+        }
+
         this.Destroy();
         Application.Quit();
     }
@@ -366,14 +369,16 @@ public partial class MainWindow : Gtk.Window
                     entryTimeout.Text = cmdManager.timeout.ToString();
                 }
 
-                // try to connect to givn server. 
+                // try to connect to given server. 
                 try
                 {
                     status = cmdManager.Open(entryServerName.Text, Convert.ToInt32(entryServerPort.Text));
+                    ClientUDP.UDPOpen(entryServerName.Text,Convert.ToInt32(entryServerPort.Text) + 1);
                 }
-                catch (Exception)
+                catch (Exception err)
                 {
                     Console.WriteLine("Something went wrong during connection");
+                    Console.WriteLine(err.ToString());
                     return;
                 }
 
@@ -401,6 +406,7 @@ public partial class MainWindow : Gtk.Window
                                      "Unable to open communication with robot.\nCheck that supervisor is accepting OPEN_COM_DMB command");
 
                         cmdManager.Close();
+                        ClientUDP.UDPClose();
                     }
                 }
             }
@@ -545,6 +551,74 @@ public partial class MainWindow : Gtk.Window
         else batteryTimer.Start();
     }
 
+    /// <summary>
+    /// Thread for image reception
+    /// </summary>
+    private void ImageThread()
+    {
+        byte[] imageFull = null;
+        byte[] imageBuffer = null;
+        byte[] bytes;
+        bool done = false;
+
+        while (true)
+        {
+            bytes = new byte[1];
+            bytes[0] = 0;
+
+            try
+            {
+                bytes = ClientUDP.GetData();
+            }
+            catch (Exception e)
+            {
+                Console.WriteLine(e.ToString());
+            }
+
+            if (bytes[0] == (byte)'I')
+            {
+                // Nouvelle trame recu
+                imageFull = imageBuffer;
+                imageBuffer = bytes;
+                done = true;
+            }
+
+            else
+            {
+                Array.Resize<byte>(ref imageBuffer, imageBuffer.Length + bytes.Length); // resize currrent buffer
+                System.Buffer.BlockCopy(imageBuffer, 0, bytes, imageBuffer.Length - bytes.Length, bytes.Length);
+            }
+
+
+            if (done)
+            {
+                done = false;
+
+                if (imageFull != null)
+                {
+                    byte[] image = new byte[imageFull.Length - 4];
+                    System.Buffer.BlockCopy(imageFull, 4, image, 0, image.Length);
+
+                    imageReceivedCounter++;
+                    try
+                    {
+                        drawingareaCameraPixbuf = new Pixbuf(image);
+                        drawingAreaCamera.QueueDraw();
+                    }
+                    catch (GLib.GException)
+                    {
+                        badImageReceivedCounter++;
+#if DEBUG
+                        Console.WriteLine("Bad Image: " + badImageReceivedCounter +
+                                          " / " + imageReceivedCounter +
+                                          " (" + badImageReceivedCounter * 100 / imageReceivedCounter + "%)");
+#endif
+                    }
+                }
+            }
+        }
+    }
+
     /// <summary>
     /// Callback called when checkbutton for camera is clicked
     /// </summary>
@@ -561,6 +635,14 @@ public partial class MainWindow : Gtk.Window
                              ButtonsType.Ok, "Error",
                              "Error when closing camera: bad answer for supervisor or timeout");
             }
+            else
+            {
+                if (imageThread != null)
+                {
+                    imageThread.Abort();
+                    imageThread = null;
+                }
+            }
         }
         else // camera is not active, switch it on
         {
@@ -574,6 +656,11 @@ public partial class MainWindow : Gtk.Window
                              "Error when opening camera: bad answer for supervisor or timeout");
                 checkButtonCameraOn.Active = false;
             }
+            else
+            {
+                imageThread = new Thread(new ThreadStart(ImageThread));
+                imageThread.Start();
+            }
         }
     }
 

software/raspberry/superviseur-robot/lib/server.h

@@ -24,5 +24,8 @@ int sendDataToServer(char *data, int length);
 int sendDataToServerForClient(int client, char *data, int length);
 int receiveDataFromServer(char *data, int size);
 int receiveDataFromServerFromClient(int client, char *data, int size);
+
+int sendCamImage(char *data, int length);
+void waitUdpPing(void);
 #endif /* _SERVER_H_ */
 

software/raspberry/superviseur-robot/lib/src/server.cpp

@@ -53,7 +53,7 @@ int openServer(int port) {
     listen(socketFD, NB_CONNECTION_MAX);
 
     /* Open UDP connection */
-    socketUDP = socket(AF_INET, SOCK_DGRAM, 0);
+    socketUDP = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
     if (socketUDP < 0) {
         perror("Can not create UDP socket");
         exit(-1);
@@ -114,33 +114,38 @@ void waitUDPClientAddr(void)
             MSG_WAITALL, ( struct sockaddr *) &UDPcliaddr, &UDPcliaddrlen); 
 }
 
-int sendImage(char *data, int length) {
+int sendCamImage(char *data, int length) {
     if (clientID >= 0)
     {
-        UDPBuffer= (char*)malloc(length+12);
+        UDPBuffer= (char*)malloc(length+4);
     
         UDPBuffer[0]='I';
-        UDPBuffer[1]='N';
+        UDPBuffer[1]='M';
-        UDPBuffer[2]='S';
+        UDPBuffer[2]='G';
-        UDPBuffer[3]='A';
+        UDPBuffer[3]=':';
-        UDPBuffer[4]='B';
-        UDPBuffer[5]='G';
         
-        UDPBuffer[length+6]='A';
+        memcpy((void*)(UDPBuffer+4),(const void *)data, length);
-        UDPBuffer[length+7]='S';
-        UDPBuffer[length+8]='N';
-        UDPBuffer[length+9]='I';
-        UDPBuffer[length+10]='E';
-        UDPBuffer[length+11]='D';
         
-        memcpy((void*)(UDPBuffer+6),(const void *)data, length);
+        return sendto(socketUDP, UDPBuffer, length+4, 
-        
-        return sendto(socketUDP, data, length, 
         MSG_CONFIRM, (const struct sockaddr *) &UDPcliaddr, UDPcliaddrlen); 
     }   
     else return 0;
 }
 
+void waitUdpPing(void)
+{
+    char msg[4];
+    volatile int i=0;
+    
+    // Wait for client to send a small message, to get its IP
+    UDPcliaddrlen = sizeof(UDPcliaddr);
+    bzero(&UDPcliaddr, sizeof(UDPcliaddr));
+    
+    recvfrom(socketUDP, msg, 2, 0, (sockaddr *)&UDPcliaddr, &UDPcliaddrlen);
+    printf("Received packet from %s:%d\n", 
+            inet_ntoa(UDPcliaddr.sin_addr), ntohs(UDPcliaddr.sin_port));
+}
+
 int receiveDataFromServer(char *data, int size) {
     return receiveDataFromServerFromClient(clientID, data, size);
 }

software/raspberry/testeur/testeur/main.cpp

@@ -206,6 +206,7 @@ int decodeMessage(MessageFromMon *mes, int dataLength) {
 
 int main(int argc, char** argv) {
 
+    int lengthSend;
     // Ouverture de la com robot
 #ifdef __FOR_PC__
     if (open_communication_robot("/dev/ttyUSB0") != 0) {
@@ -233,6 +234,10 @@ int main(int argc, char** argv) {
     socketID = openServer(5544);
     cout << "Server opened on port 5544";
     cout << std::endl;
+    cout << "UDP Server opened on port 5545";
+    cout << std::endl;
+
+    waitUdpPing();
     
     threadTimer = new std::thread(ThreadTimer);
 
@@ -271,8 +276,12 @@ int main(int argc, char** argv) {
                 if (sendImage) {
                     compress_image(&monImage, &imageCompressed);
                     int length = imageCompressed.size();
-                    sendBinaryData(HEADER_STM_IMAGE, reinterpret_cast<char*> (imageCompressed.data()), length);
+                    //sendBinaryData(HEADER_STM_IMAGE, reinterpret_cast<char*> (imageCompressed.data()), length);
                     //sendAnswer(HEADER_STM_IMAGE, reinterpret_cast<char*> (imageCompressed.data()));
+                    lengthSend=sendCamImage(reinterpret_cast<char*> (imageCompressed.data()), length);
+                    
+                    cout << "Requested Length: " + to_string(length) + " / Send Length: " + to_string(lengthSend);
+                    cout << std::endl;
                 }
 
                 if (sendPos) {

software/raspberry/testeur/testeur/nbproject/private/private.xml

@@ -7,17 +7,13 @@
     <editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
     <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
         <group>
-<<<<<<< HEAD
-            <file>file:/home/dimercur/Documents/Travail/dumber/software/raspberry/superviseur-robot/lib/src/server.cpp</file>
-            <file>file:/home/dimercur/Documents/Travail/dumber/software/raspberry/superviseur-robot/lib/src/image.cpp</file>
-            <file>file:/home/dimercur/Documents/Travail/dumber/software/raspberry/testeur/testeur/main.cpp</file>
-=======
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/image.h</file>
+            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h</file>
+            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/server.cpp</file>
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/image.cpp</file>
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/robot.cpp</file>
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/main.cpp</file>
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/robot.h</file>
->>>>>>> dev
         </group>
     </open-files>
 </project-private>

software/raspberry/testeur/testeur/nbproject/private/timestamps-10.105.1.13-pi-22

@@ -1,26 +1,26 @@
-#Mon Nov 12 11:20:04 CET 2018
+#Mon Nov 12 17:04:27 CET 2018
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/robot.h=c1541685829000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/project.xml=c1542014288000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/project.xml=c1542029322000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Makefile-impl.mk=c1542014288000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Makefile-impl.mk=c1542029322000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/robot.cpp=c1541685829000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/image.h=c1541685829000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/main.cpp=c1542017998000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/main.cpp=c1542036786000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/server.cpp=c1542014288000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/server.cpp=c1542038373000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Makefile-Release.mk=c1541685829000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/message.cpp=c1541685829000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Package-Debug.bash=c1541685829000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/.gitignore=c1541685829000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Makefile-variables.mk=c1542014288000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Makefile-variables.mk=c1542029322000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/private/Makefile-variables.mk=c1542014288000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/private/Makefile-variables.mk=c1542029322000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/Makefile=c1541685829000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Makefile-Debug-rpi.mk=c1542014288000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Makefile-Debug-rpi.mk=c1542029322000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Makefile-Debug.mk=c1541775193000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/message.h=c1541685829000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/image.cpp=c1542014288000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/image.cpp=c1542029322000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Package-Release.bash=c1541685829000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/.dep.inc=c1541685829000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/.dep.inc=c1542035117000
 VERSION=1.3
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h=c1541685829000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h=c1542036764000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/definitions.h=c1541685829000
-/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Package-Debug-rpi.bash=c1542014288000
+/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/nbproject/Package-Debug-rpi.bash=c1542029322000
 /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/monitor.cpp=c1541685829000