diff --git a/software/monitor/monitor/Client.cs b/software/monitor/monitor/Client.cs index 48bb9c9..a5de13b 100644 --- a/software/monitor/monitor/Client.cs +++ b/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; diff --git a/software/monitor/monitor/ClientUDP.cs b/software/monitor/monitor/ClientUDP.cs index 2ba7ae1..3b9d7ab 100644 --- a/software/monitor/monitor/ClientUDP.cs +++ b/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 IPEndPoint groupEP = null; + private static UdpClient clientUDP = null; + private static IPEndPoint ep = null; - public static void UDPOpen(int port) + public static void UDPOpen(string addr, int port) { - listener = new UdpClient(port); - groupEP = new IPEndPoint(IPAddress.Any, port); + clientUDP = new UdpClient(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 - { - while (!done) - { - Console.WriteLine("Waiting for broadcast"); - byte[] bytes = listener.Receive(ref groupEP); + Console.WriteLine("Received broadcast from {0} :\n {1}\n", + ep.ToString(), + Encoding.ASCII.GetString(bytes, 0, bytes.Length)); + + return bytes; + } - Console.WriteLine("Received broadcast from {0} :\n {1}\n", - groupEP.ToString(), - Encoding.ASCII.GetString(bytes, 0, bytes.Length)); + public static bool SendPing() + { + byte[] msg = new byte[2]; + msg[0] = (byte)'O'; + msg[1] = (byte)'k'; - if (bytes[0]=='I') { - // Nouvelle trame recu - ImageBuffer = bytes; - } - else if (bytes[bytes.Length-1]=='D') - { - Array.Resize(ref ImageBuffer, ImageBuffer.Length + bytes.Length); // resize currrent buffer + Console.WriteLine("Ping Server to send address"); + clientUDP.Send(msg, msg.Length); - System.Buffer.BlockCopy(ImageBuffer, 0, bytes, ImageBuffer.Length-bytes.Length, bytes.Length); - done = true; - } - else{ - Array.Resize(ref ImageBuffer, ImageBuffer.Length + bytes.Length); // resize currrent buffer - - System.Buffer.BlockCopy(ImageBuffer, 0, bytes, ImageBuffer.Length-bytes.Length, bytes.Length); - } - } - - } - catch (Exception e) - { - Console.WriteLine(e.ToString()); - } - - return ImageBuffer; + return true; } } } diff --git a/software/monitor/monitor/MonitorUI.cs b/software/monitor/monitor/MonitorUI.cs index d042c15..82a3e10 100644 --- a/software/monitor/monitor/MonitorUI.cs +++ b/software/monitor/monitor/MonitorUI.cs @@ -24,6 +24,8 @@ using System; using Gtk; using Gdk; +using System.Threading; + using monitor; /// @@ -61,9 +63,28 @@ public partial class MainWindow : Gtk.Window /// private System.Timers.Timer batteryTimer; + /// + /// Counter for total image received, reset after CAM_OPEN is sent + /// private int imageReceivedCounter = 0; + + /// + /// Counter increased each time incorrect image is received + /// private int badImageReceivedCounter = 0; + /// + /// Buffer containing a complete image. + /// + private byte[] imageComplete; + + /// + /// Buffer used to store incoming image. + /// + private byte[] imageInProgress; + + private Thread imageThread; + /// /// Initializes a new instance of the class. /// @@ -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; - /// /// Callback called when new message is received from server /// @@ -211,7 +238,7 @@ public partial class MainWindow : Gtk.Window /// Raw buffer corresponding of received message 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(); } + /// + /// Thread for image reception + /// + 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(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 + } + } + } + } + } + /// /// Callback called when checkbutton for camera is clicked /// @@ -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(); + } } } diff --git a/software/monitor/monitor/monitor b/software/monitor/monitor/monitor index d65f28c..d6b81c1 100755 Binary files a/software/monitor/monitor/monitor and b/software/monitor/monitor/monitor differ diff --git a/software/raspberry/superviseur-robot/lib/server.h b/software/raspberry/superviseur-robot/lib/server.h index 31904db..b571960 100644 --- a/software/raspberry/superviseur-robot/lib/server.h +++ b/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_ */ diff --git a/software/raspberry/superviseur-robot/lib/src/server.cpp b/software/raspberry/superviseur-robot/lib/src/server.cpp index 85b65a4..ee3fa26 100644 --- a/software/raspberry/superviseur-robot/lib/src/server.cpp +++ b/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[2]='S'; - UDPBuffer[3]='A'; - UDPBuffer[4]='B'; - UDPBuffer[5]='G'; + UDPBuffer[1]='M'; + UDPBuffer[2]='G'; + UDPBuffer[3]=':'; - UDPBuffer[length+6]='A'; - UDPBuffer[length+7]='S'; - UDPBuffer[length+8]='N'; - UDPBuffer[length+9]='I'; - UDPBuffer[length+10]='E'; - UDPBuffer[length+11]='D'; + memcpy((void*)(UDPBuffer+4),(const void *)data, length); - memcpy((void*)(UDPBuffer+6),(const void *)data, length); - - return sendto(socketUDP, 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); } diff --git a/software/raspberry/testeur/testeur/main.cpp b/software/raspberry/testeur/testeur/main.cpp index 3bdccc8..1dba719 100644 --- a/software/raspberry/testeur/testeur/main.cpp +++ b/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 (imageCompressed.data()), length); + //sendBinaryData(HEADER_STM_IMAGE, reinterpret_cast (imageCompressed.data()), length); //sendAnswer(HEADER_STM_IMAGE, reinterpret_cast (imageCompressed.data())); + lengthSend=sendCamImage(reinterpret_cast (imageCompressed.data()), length); + + cout << "Requested Length: " + to_string(length) + " / Send Length: " + to_string(lengthSend); + cout << std::endl; } if (sendPos) { diff --git a/software/raspberry/testeur/testeur/nbproject/private/private.xml b/software/raspberry/testeur/testeur/nbproject/private/private.xml index e7af67d..1d5cbc6 100644 --- a/software/raspberry/testeur/testeur/nbproject/private/private.xml +++ b/software/raspberry/testeur/testeur/nbproject/private/private.xml @@ -7,17 +7,13 @@ -<<<<<<< HEAD - file:/home/dimercur/Documents/Travail/dumber/software/raspberry/superviseur-robot/lib/src/server.cpp - file:/home/dimercur/Documents/Travail/dumber/software/raspberry/superviseur-robot/lib/src/image.cpp - file:/home/dimercur/Documents/Travail/dumber/software/raspberry/testeur/testeur/main.cpp -======= file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/image.h + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/server.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/image.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/src/robot.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/testeur/testeur/main.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/robot.h ->>>>>>> dev diff --git a/software/raspberry/testeur/testeur/nbproject/private/timestamps-10.105.1.13-pi-22 b/software/raspberry/testeur/testeur/nbproject/private/timestamps-10.105.1.13-pi-22 index 727d827..ae04fb4 100644 --- a/software/raspberry/testeur/testeur/nbproject/private/timestamps-10.105.1.13-pi-22 +++ b/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