UDP a peu pres OK

This commit is contained in:
Sébastien DI MERCURIO 2018-11-12 17:13:52 +01:00
parent 8bbe8c8450
commit 443335431b
9 changed files with 209 additions and 119 deletions

View file

@ -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;

View file

@ -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));
Console.WriteLine("Received broadcast from {0} :\n {1}\n",
groupEP.ToString(),
Encoding.ASCII.GetString(bytes, 0, bytes.Length));
return bytes;
}
if (bytes[0]=='I') {
// Nouvelle trame recu
ImageBuffer = bytes;
}
else if (bytes[bytes.Length-1]=='D')
{
Array.Resize<byte>(ref ImageBuffer, ImageBuffer.Length + bytes.Length); // resize currrent buffer
public static bool SendPing()
{
byte[] msg = new byte[2];
msg[0] = (byte)'O';
msg[1] = (byte)'k';
System.Buffer.BlockCopy(ImageBuffer, 0, bytes, ImageBuffer.Length-bytes.Length, bytes.Length);
done = true;
}
else{
Array.Resize<byte>(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);
}
}
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
}
return ImageBuffer;
return true;
}
}
}

View file

@ -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();
}
}
}

Binary file not shown.

View file

@ -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_ */

View file

@ -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);
}

View file

@ -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) {

View file

@ -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>

View file

@ -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