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

@@ -0,0 +1,54 @@
+using System;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+
+namespace monitor
+{
+    public static class ClientUDP
+    {
+        private const int listenPort = 11000;
+
+        private static UdpClient clientUDP = null;
+        private static IPEndPoint ep = null;
+
+        public static void UDPOpen(string addr, int port)
+        {
+            clientUDP = new UdpClient(port);
+            ep = new IPEndPoint(IPAddress.Parse(addr), port);
+            clientUDP.Connect(ep);
+
+            SendPing();
+        }
+
+        public static void UDPClose()
+        {
+            if (clientUDP!=null)
+                clientUDP.Close();
+        }
+
+        public static byte[] GetData()
+        {
+            Console.WriteLine("Waiting for broadcast");
+            byte[] bytes = clientUDP.Receive(ref ep);
+
+            Console.WriteLine("Received broadcast from {0} :\n {1}\n",
+                ep.ToString(),
+                Encoding.ASCII.GetString(bytes, 0, bytes.Length));
+            
+            return bytes;
+        }
+
+        public static bool SendPing()
+        {
+            byte[] msg = new byte[2];
+            msg[0] = (byte)'O';
+            msg[1] = (byte)'k';
+
+            Console.WriteLine("Ping Server to send address");
+            clientUDP.Send(msg, msg.Length);
+
+            return true;
+        }
+    }
+}

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();
                     }
                 }
             }
@@ -417,10 +423,10 @@ public partial class MainWindow : Gtk.Window
         DestijlCommandManager.CommandStatus status;
 
         //if robot is not activated
-        if (buttonRobotActivation.Label == "Activate") 
+        if (buttonRobotActivation.Label == "Activate")
         {
             // if a startup with watchdog is requested
-            if (radioButtonWithWatchdog.Active) 
+            if (radioButtonWithWatchdog.Active)
             {
                 status = cmdManager.RobotStartWithWatchdog();
             }
@@ -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/monitor/monitor/monitor.csproj

@@ -80,6 +80,7 @@
     <Compile Include="Client.cs" />
     <Compile Include="CommandManager.cs" />
     <Compile Include="DestijlCommandManager.cs" />
+    <Compile Include="ClientUDP.cs" />
   </ItemGroup>
   <ItemGroup>
     <None Include="afterBuild.sh" />

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

@@ -19,15 +19,22 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <string.h>
 
 #define NB_CONNECTION_MAX 1
 
 int socketFD = -1;
 int clientID = -1;
 
+int socketUDP= -1;
+struct sockaddr_in UDPcliaddr;
+socklen_t UDPcliaddrlen = -1;
+char *UDPBuffer=0;
+
 int openServer(int port) {
     struct sockaddr_in server;
-
+    struct sockaddr_in serverUDP;
+    
     socketFD = socket(AF_INET, SOCK_STREAM, 0);
     if (socketFD < 0) {
         perror("Can not create socket");
@@ -45,13 +52,33 @@ int openServer(int port) {
 
     listen(socketFD, NB_CONNECTION_MAX);
 
+    /* Open UDP connection */
+    socketUDP = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+    if (socketUDP < 0) {
+        perror("Can not create UDP socket");
+        exit(-1);
+    }
+    
+    serverUDP.sin_addr.s_addr = INADDR_ANY;
+    serverUDP.sin_family = AF_INET;
+    serverUDP.sin_port = htons(port+1);
+    
+    memset(&UDPcliaddr, 0, sizeof(UDPcliaddr)); 
+    
+    if (bind(socketUDP, (struct sockaddr *) &serverUDP, sizeof (serverUDP)) < 0) {
+        perror("Can not bind UDP socket");
+        exit(-1);
+    }
+    
     return socketFD;
 }
 
 int closeServer() {
     close(socketFD);
-
+    close(socketUDP);
+    
     socketFD = -1;
+    socketUDP = -1;
     
     return 0;
 }
@@ -80,6 +107,45 @@ int sendDataToServerForClient(int client, char *data, int length) {
     else return 0;
 }
 
+void waitUDPClientAddr(void)
+{
+    char buffer[10]; 
+    recvfrom(socketUDP, (char *)buffer, 10,  
+            MSG_WAITALL, ( struct sockaddr *) &UDPcliaddr, &UDPcliaddrlen); 
+}
+
+int sendCamImage(char *data, int length) {
+    if (clientID >= 0)
+    {
+        UDPBuffer= (char*)malloc(length+4);
+    
+        UDPBuffer[0]='I';
+        UDPBuffer[1]='M';
+        UDPBuffer[2]='G';
+        UDPBuffer[3]=':';
+        
+        memcpy((void*)(UDPBuffer+4),(const void *)data, length);
+        
+        return sendto(socketUDP, UDPBuffer, length+4, 
+        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,7 +234,11 @@ 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);
 
     for (;;) {
@@ -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

@@ -8,6 +8,8 @@
     <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
         <group>
             <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>

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/Makefile-impl.mk=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=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/superviseur-robot/lib/src/server.cpp=c1542014288000
+/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=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/private/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=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