diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index ee202d8..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "configurations": [ - { - "name": "Linux", - "includePath": [ - "${workspaceFolder}/**" - ], - "defines": [], - "compilerPath": "/usr/bin/clang", - "cStandard": "c11", - "cppStandard": "c++17", - "intelliSenseMode": "clang-x64" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json deleted file mode 100644 index 29327bf..0000000 --- a/.vscode/tasks.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - // See https://go.microsoft.com/fwlink/?LinkId=733558 - // for the documentation about the tasks.json format - "version": "2.0.0", - "tasks": [ - { - "taskName": "Compile on raspberry", - "command": "rsync -az '${file}' 10.105.1.6:~ && ssh server.example.org 'chmod +x ./${fileBasename}; ./${fileBasename}'", - "type": "shell", - "group": { - "kind": "build", - "isDefault": true - } - } - ] -} \ No newline at end of file diff --git a/software/monitor/monitor/Client.cs b/software/monitor/monitor/Client.cs index 48bb9c9..b6afe8a 100644 --- a/software/monitor/monitor/Client.cs +++ b/software/monitor/monitor/Client.cs @@ -23,6 +23,8 @@ using System; using System.Net.Sockets; using System.Text; +using System.Threading; + namespace monitor { /// @@ -66,21 +68,26 @@ namespace monitor /// private static byte[] receiveBuffer; - private static int initialReceiveBufferIndex = 0; + //private static int initialReceiveBufferIndex = 0; /// /// String containing received message from tcp server /// private static StringBuilder message = new StringBuilder(); - private static int newLength = 1; - private static int packetCounter = 0; + //private static int newLength = 1; + //private static int packetCounter = 0; /// /// Callback to send received message to upper level /// - public delegate void ReadEvent(string msg, byte[] buffer); + public delegate void ReadEvent(string msg); public static ReadEvent readEvent = null; + /// + /// Thread used in reception + /// + private static Thread readThread; + /// /// Open connection to server "host", on default port number. /// @@ -114,7 +121,12 @@ namespace monitor // received data are stored in buffer // Next reading will be done in ReadCallback method - stream.BeginRead(buffer, 0, newLength, new AsyncCallback(ReadCallback), message); + stream.BeginRead(buffer, 0, 1, new AsyncCallback(ReadCallback), message); + + // Start reading thread + //message.Clear(); + //readThread = new Thread(new ThreadStart(ReadCallback)); + //readThread.Start(); } catch (ArgumentNullException e) { @@ -144,19 +156,142 @@ namespace monitor if (client != null) client.Close(); } + /// + /// Callback call by stream.BeginRead after reception of newLength data + /// + //private static void ReadCallback() + //{ + // char character; + // int data; + // byte[] buffer=new byte[4096]; + // int lengthRead; + + // while (client.Connected) + // { + // try + // { + // //data = stream.ReadByte(); + // lengthRead = stream.Read(buffer, 0, buffer.Length); + // } + // catch (ObjectDisposedException) + // { + // Console.WriteLine("Connection to server dropped (object disposed)!"); + // return; + // } + // catch (System.IO.IOException) + // { + // Console.WriteLine("Connection to server dropped (other error)"); + // return; + // } + + // if (lengthRead > 0) // a data was read + // { + // //character = (char)data; + // var str = System.Text.Encoding.Default.GetString(buffer); + // int indexOf = str.IndexOf('\n'); + + // //if (character != '\n') + // if (indexOf == -1) // \n doesn't exists + // { + // message.Append(str); + + // //if (receiveBuffer == null) receiveBuffer = new byte[1]; + // //else Array.Resize(ref receiveBuffer, receiveBuffer.Length + 1); // resize currrent buffer + + // //receiveBuffer[receiveBuffer.Length - 1] = (byte)data; + // } + // else // end of string found + // { + // message.Append((str.Substring(0,indexOf))); + // readEvent?.Invoke(message.ToString(), receiveBuffer); + + // message.Clear(); + // receiveBuffer = null; + // } + // } + // } + //} + /// /// Callback call by stream.BeginRead after reception of newLength data /// /// Not sure of what is it, but needed for terminate reading + //private static void ReadCallback(IAsyncResult ar) + //{ + // if (client.Connected) + // { + // int bytesRead; + + // try + // { + // // Termintae read operation, and get number of byte stored in buffer + // bytesRead = stream.EndRead(ar); + // } + // catch (ObjectDisposedException e) + // { + // Console.WriteLine("Connection to server dropped: " + e.ToString()); + // return; + // } + + // newLength = 1; + + // // if number of byte read is not 0, concatenate string and buffer + // if (bytesRead > 0) + // { + // packetCounter++; + + // if (packetCounter >= 3) + // { + // //Console.WriteLine("Supplementary packet " + packetCounter); + // } + + // // Append new data to current string (expecting data are ascii) + // message.Append(Encoding.ASCII.GetString(buffer, 0, bytesRead)); + + // // Similarly, append received bytes to current buffer + // if (receiveBuffer == null) receiveBuffer = new byte[bytesRead]; + // else Array.Resize(ref receiveBuffer, initialReceiveBufferIndex + bytesRead); // resize currrent buffer + + // System.Buffer.BlockCopy(buffer, 0, receiveBuffer, initialReceiveBufferIndex, bytesRead); // and add received data + // initialReceiveBufferIndex = receiveBuffer.Length; // move last index of current buffer + // } + + // // if it remains received data, prepare for a new reading (get another buffer to append to current one) + // if (client.Available > 0) + // { + // newLength = client.Available; + // if (newLength > BufferMaxSize) newLength = BufferMaxSize; + // else newLength = client.Available; + // } + // else + // { + // // no more data to read, buffer and string can be send to upper level + // readEvent?.Invoke(message.ToString(), receiveBuffer); + + // message.Clear(); + // receiveBuffer = null; + // initialReceiveBufferIndex = 0; + // packetCounter = 0; + // } + + // // Prepare for reading new data + // if (newLength> BufferMaxSize) newLength = BufferMaxSize; + // if (newLength <= 0) newLength = 1; + // stream.BeginRead(buffer, 0, newLength, new AsyncCallback(ReadCallback), message); + // } + //} + private static void ReadCallback(IAsyncResult ar) { + int newLength = 1; + if (client.Connected) { int bytesRead; try { - // Termintae read operation, and get number of byte stored in buffer + // Terminate read operation, and get number of byte stored in buffer bytesRead = stream.EndRead(ar); } catch (ObjectDisposedException e) @@ -165,27 +300,13 @@ namespace monitor return; } - newLength = 1; + //newLength = 1; // if number of byte read is not 0, concatenate string and buffer if (bytesRead > 0) { - packetCounter++; - - if (packetCounter >= 3) - { - //Console.WriteLine("Supplementary packet " + packetCounter); - } - // Append new data to current string (expecting data are ascii) message.Append(Encoding.ASCII.GetString(buffer, 0, bytesRead)); - - // Similarly, append received bytes to current buffer - if (receiveBuffer == null) receiveBuffer = new byte[bytesRead]; - else Array.Resize(ref receiveBuffer, initialReceiveBufferIndex + bytesRead); // resize currrent buffer - - System.Buffer.BlockCopy(buffer, 0, receiveBuffer, initialReceiveBufferIndex, bytesRead); // and add received data - initialReceiveBufferIndex = receiveBuffer.Length; // move last index of current buffer } // if it remains received data, prepare for a new reading (get another buffer to append to current one) @@ -198,12 +319,9 @@ namespace monitor else { // no more data to read, buffer and string can be send to upper level - readEvent?.Invoke(message.ToString(), receiveBuffer); + readEvent?.Invoke(message.ToString()); message.Clear(); - receiveBuffer = null; - initialReceiveBufferIndex = 0; - packetCounter = 0; } // Prepare for reading new data diff --git a/software/monitor/monitor/CommandManager.cs b/software/monitor/monitor/CommandManager.cs index 803e229..03efac1 100644 --- a/software/monitor/monitor/CommandManager.cs +++ b/software/monitor/monitor/CommandManager.cs @@ -33,7 +33,7 @@ namespace monitor /// /// Callback for sending received data to upper level /// - public delegate void CommandReceivedEvent(string msg, byte[] buffer); + public delegate void CommandReceivedEvent(string msg); public CommandReceivedEvent commandReceivedEvent = null; /// @@ -122,7 +122,7 @@ namespace monitor /// /// Message received from server /// Raw buffer reived from server - private void OnMessageReception(string message, byte[] buffer) + private void OnMessageReception(string message) { waitTimer.Stop(); // Stop timeout stopwatch @@ -144,7 +144,7 @@ namespace monitor waitForAcknowledge = false; - this.commandReceivedEvent?.Invoke(message, buffer); + this.commandReceivedEvent?.Invoke(message); } } @@ -158,7 +158,7 @@ namespace monitor messageReceived = null; // set buffer and message as null to indicate that no message was received // and call to OnMessagereception is due to timeout - OnMessageReception(messageReceived, null); + OnMessageReception(messageReceived); } /// diff --git a/software/monitor/monitor/DestijlCommandManager.cs b/software/monitor/monitor/DestijlCommandManager.cs index 2f85469..3257eb8 100644 --- a/software/monitor/monitor/DestijlCommandManager.cs +++ b/software/monitor/monitor/DestijlCommandManager.cs @@ -20,6 +20,7 @@ // along with this program. If not, see . using System; +using System.Globalization; namespace monitor { @@ -28,47 +29,84 @@ namespace monitor /// public static class DestijlCommandList { - public const string HeaderMtsComDmb = "COM"; - public const string HeaderMtsDmbOrder = "DMB"; - public const string HeaderMtsCamera = "CAM"; - public const string HeaderMtsMessage = "MSG"; + public const string ANSWER_ACK = "AACK"; + public const string ANSWER_NACK = "ANAK"; + public const string ANSWER_COM_ERROR = "ACER"; + public const string ANSWER_TIMEOUT = "ATIM"; + public const string ANSWER_CMD_REJECTED = "ACRJ"; + public const string MESSAGE = "MSSG"; + public const string CAMERA_OPEN = "COPN"; + public const string CAMERA_CLOSE = "CCLS"; + public const string CAMERA_IMAGE = "CIMG"; + public const string CAMERA_ARENA_ASK = "CASA"; + public const string CAMERA_ARENA_INFIRM = "CAIN"; + public const string CAMERA_ARENA_CONFIRM = "CACO"; + public const string CAMERA_POSITION_COMPUTE = "CPCO"; + public const string CAMERA_POSITION_STOP = "CPST"; + public const string CAMERA_POSITION = "CPOS"; + public const string ROBOT_COM_OPEN = "ROPN"; + public const string ROBOT_COM_CLOSE = "RCLS"; + public const string ROBOT_PING = "RPIN"; + public const string ROBOT_RESET = "RRST"; + public const string ROBOT_START_WITHOUT_WD = "RSOW"; + public const string ROBOT_START_WITH_WD = "RSWW"; + public const string ROBOT_RELOAD_WD = "RLDW"; + public const string ROBOT_MOVE = "RMOV"; + public const string ROBOT_TURN = "RTRN"; + public const string ROBOT_GO_FORWARD = "RGFW"; + public const string ROBOT_GO_BACKWARD = "RGBW"; + public const string ROBOT_GO_LEFT = "RGLF"; + public const string ROBOT_GO_RIGHT = "RGRI"; + public const string ROBOT_STOP = "RSTP"; + public const string ROBOT_POWEROFF = "RPOF"; + public const string ROBOT_BATTERY_LEVEL = "RBLV"; + public const string ROBOT_GET_BATTERY = "RGBT"; + public const string ROBOT_GET_STATE = "RGST"; + public const string ROBOT_CURRENT_STATE = "RCST"; - public const string DataComOpen = "o"; - public const string DataComClose = "C"; + public const char SEPARATOR_CHAR = ':'; - public const string DataCamOpen = "A"; - public const string DataCamClose = "I"; - public const string DataCamAskArena = "y"; - public const string DataCamArenaConfirm = "x"; - public const string DataCamInfirm = "z"; - public const string DataCamComputePosition = "p"; - public const string DataCamStopComputePosition = "s"; + //public const string HeaderMtsComDmb = "COM"; + //public const string HeaderMtsDmbOrder = "DMB"; + //public const string HeaderMtsCamera = "CAM"; + //public const string HeaderMtsMessage = "MSG"; - public const string HeaderStmAck = "ACK"; - public const string HeaderStmNoAck = "NAK"; - public const string HeaderStmLostDmb = "LCD"; - public const string HeaderStmImage = "IMG"; - public const string HeaderStmPos = "POS"; - public const string HeaderStmMes = "MSG"; - public const string HeaderStmBat = "BAT"; + //public const string DataComOpen = "o"; + //public const string DataComClose = "C"; + + //public const string DataCamOpen = "A"; + //public const string DataCamClose = "I"; + //public const string DataCamAskArena = "y"; + //public const string DataCamArenaConfirm = "x"; + //public const string DataCamInfirm = "z"; + //public const string DataCamComputePosition = "p"; + //public const string DataCamStopComputePosition = "s"; + + //public const string HeaderStmAck = "ACK"; + //public const string HeaderStmNoAck = "NAK"; + //public const string HeaderStmLostDmb = "LCD"; + //public const string HeaderStmImage = "IMG"; + //public const string HeaderStmPos = "POS"; + //public const string HeaderStmMes = "MSG"; + //public const string HeaderStmBat = "BAT"; } /// /// Commands used for robot messages /// - public static class RobotCommandList - { - public const string RobotPing = "p"; - public const string RobotReset = "r"; - public const string RobotStartWithoutWatchdog = "u"; - public const string RobotStartWithWatchdog = "W"; - public const string RobotGetBattery = "v"; - public const string RobotGetBusyState = "b"; - public const string RobotMove = "M"; - public const string RobotTurn = "T"; - public const string RobotGetVersion = "V"; - public const string RobotPowerOff = "z"; - } + //public static class RobotCommandList + //{ + // public const string RobotPing = "p"; + // public const string RobotReset = "r"; + // public const string RobotStartWithoutWatchdog = "u"; + // public const string RobotStartWithWatchdog = "W"; + // public const string RobotGetBattery = "v"; + // public const string RobotGetBusyState = "b"; + // public const string RobotMove = "M"; + // public const string RobotTurn = "T"; + // public const string RobotGetVersion = "V"; + // public const string RobotPowerOff = "z"; + //} /// /// Specialization class for command manager, which implemnent destijl protocol between monitor and server @@ -93,7 +131,7 @@ namespace monitor /// /// Callback for sending received data to application level /// - public delegate void CommandReceivedEvent(string header, string data, byte[] buffer); + public delegate void CommandReceivedEvent(string header, string data); public CommandReceivedEvent commandReceivedEvent = null; /// @@ -114,6 +152,34 @@ namespace monitor CommunicationLostWithServer } + public struct Point { + public double x; + public double y; + } + + public class Position { + public int robotID; + public double angle; + public Point centre; + public Point direction; + + public Position() { + robotID = 0; + angle = 0.0; + centre.x = 0.0; + centre.y = 0.0; + direction.x = 0.0; + direction.y = 0.0; + } + + public override string ToString() { + string s = "ID: " + robotID + ", Angle: " + angle + + ", Centre (x: " + centre.x + ", y: " + centre.y + + "), Direction (x: " + direction.x + ", y: " + direction.y + ")"; + return s; + } + } + /// /// Initializes a new instance of the class. /// @@ -138,10 +204,13 @@ namespace monitor /// /// String containing received message /// Raw buffer to be used when data are not in ascii format (image for example) - private void OnCommandReceived(string msg, byte[] buffer) + private void OnCommandReceived(string msg) { - // Firstly, split message in (at least) two part : header, and data - string[] msgs = msg.Split(':'); + // Firstly, remove ending \n and everything after + string[] msgsCarriageReturn = msg.Split('\n'); + + // Second, split message in (at least) two part : header, and data + string[] msgs = msgsCarriageReturn[0].Split(DestijlCommandList.SEPARATOR_CHAR); // If it exist at least on element in string array, it should be command header if (msgs.Length >= 1) receivedHeader = msgs[0]; @@ -152,7 +221,7 @@ namespace monitor else receivedData = null; // when split is done, provide data to application - this.commandReceivedEvent?.Invoke(receivedHeader, receivedData, buffer); + this.commandReceivedEvent?.Invoke(receivedHeader, receivedData); } /// @@ -193,7 +262,17 @@ namespace monitor /// Data part of the command private string CreateCommand(string header, string data) { - return header + ":" + data; + return header + DestijlCommandList.SEPARATOR_CHAR + data+"\n"; + } + + /// + /// Creates the command to send to server, based on header + /// + /// The command string + /// Header part of the command + private string CreateCommand(string header) + { + return header + DestijlCommandList.SEPARATOR_CHAR+"\n"; } /// @@ -215,9 +294,9 @@ namespace monitor if (answer != null) { // if command is not acknowledged, return Rejected - if (answer.ToUpper().Contains(DestijlCommandList.HeaderStmNoAck)) status = CommandStatus.Rejected; + if (answer.ToUpper().Contains(DestijlCommandList.ANSWER_NACK)) status = CommandStatus.Rejected; // if communication is lost with robot, return CommunicationLostWithRobot - else if (answer.ToUpper().Contains(DestijlCommandList.HeaderStmLostDmb)) status = CommandStatus.CommunicationLostWithRobot; + else if (answer.ToUpper().Contains(DestijlCommandList.ANSWER_TIMEOUT)) status = CommandStatus.CommunicationLostWithRobot; // if answer is empty, communication with robot is lost else if (answer.Length == 0) status = CommandStatus.CommunicationLostWithServer; //else status = CommandStatus.InvalidAnswer; @@ -237,7 +316,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsComDmb, DestijlCommandList.DataComOpen), + CreateCommand(DestijlCommandList.ROBOT_COM_OPEN), out answer, this.timeout); @@ -254,7 +333,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsComDmb, DestijlCommandList.DataComClose), + CreateCommand(DestijlCommandList.ROBOT_COM_CLOSE), out answer, this.timeout); @@ -271,7 +350,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotPing), + CreateCommand(DestijlCommandList.ROBOT_PING), out answer, this.timeout); @@ -288,7 +367,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotReset), + CreateCommand(DestijlCommandList.ROBOT_RESET), out answer, 0); @@ -305,7 +384,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotStartWithWatchdog), + CreateCommand(DestijlCommandList.ROBOT_START_WITH_WD), out answer, this.timeout); @@ -322,7 +401,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotStartWithoutWatchdog), + CreateCommand(DestijlCommandList.ROBOT_START_WITHOUT_WD), out answer, this.timeout); @@ -340,7 +419,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotMove + "=" + distance), + CreateCommand(DestijlCommandList.ROBOT_MOVE, Convert.ToString(distance)), out answer, 0); @@ -358,7 +437,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotTurn + "=" + angle), + CreateCommand(DestijlCommandList.ROBOT_TURN, Convert.ToString(angle)), out answer, 0); @@ -375,48 +454,13 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotGetBattery), + CreateCommand(DestijlCommandList.ROBOT_GET_BATTERY), out answer, 0); return DecodeStatus(localStatus, answer); } - /// - /// Request robot firmware version - /// - /// Command status (see DecodeStatus) - /// todo - public CommandStatus RobotGetVersion(out string version) - { - CommandManager.CommandManagerStatus localStatus; - CommandStatus status = CommandStatus.Success; - version = ""; - - string answer; - - localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotGetVersion), - out answer, - this.timeout); - - if (localStatus == CommandManager.CommandManagerStatus.AnswerReceived) - { - string[] msg = answer.Split(':'); - - if (msg.Length > 1) - { - version = msg[1]; - } - } - else if (localStatus == CommandManager.CommandManagerStatus.Timeout) - { - status = CommandStatus.CommunicationLostWithServer; - } - - return status; - } - /// /// Power off robot /// @@ -427,7 +471,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotPowerOff), + CreateCommand(DestijlCommandList.ROBOT_POWEROFF), out answer, 0); @@ -444,7 +488,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamOpen), + CreateCommand(DestijlCommandList.CAMERA_OPEN), out answer, this.timeout); @@ -461,7 +505,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamClose), + CreateCommand(DestijlCommandList.CAMERA_CLOSE), out answer, 0); @@ -478,7 +522,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamAskArena), + CreateCommand(DestijlCommandList.CAMERA_ARENA_ASK), out answer, 0); @@ -495,7 +539,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamArenaConfirm), + CreateCommand(DestijlCommandList.CAMERA_ARENA_CONFIRM), out answer, 0); @@ -512,7 +556,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamInfirm), + CreateCommand(DestijlCommandList.CAMERA_ARENA_INFIRM), out answer, 0); @@ -529,7 +573,7 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamComputePosition), + CreateCommand(DestijlCommandList.CAMERA_POSITION_COMPUTE), out answer, 0); @@ -546,11 +590,86 @@ namespace monitor string answer; localStatus = commandManager.SendCommand( - CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamStopComputePosition), + CreateCommand(DestijlCommandList.CAMERA_POSITION_STOP), out answer, 0); return DecodeStatus(localStatus, answer); } + + public static Position DecodePosition(string data) { + Position pos = new Position(); + + pos.robotID = 0; + pos.angle = 0.0; + pos.centre.x = 0.0; + pos.centre.y=0.0; + pos.direction.x = 0.0; + pos.direction.y = 0.0; + + string[] parts = data.Split(';'); + + //for (int i = 0; i < parts.Length; i++) { + // Console.WriteLine(parts[i]); + //} + + NumberFormatInfo provider = new NumberFormatInfo(); + provider.NumberDecimalSeparator = "."; + provider.NumberGroupSeparator = ","; + provider.NumberGroupSizes = new int[] { 3 }; + + if (parts.Length == 6) { + pos.robotID = Convert.ToInt32(parts[0]); + + try + { + pos.angle = Convert.ToDouble(parts[1]); + } catch (FormatException) + { + pos.angle = Convert.ToDouble(parts[1],provider); + } + + try + { + pos.centre.x = Convert.ToDouble(parts[2]); + } catch (FormatException) + { + pos.centre.x = Convert.ToDouble(parts[2], provider); + } + + try + { + pos.centre.y = Convert.ToDouble(parts[3]); + } + catch (FormatException) + { + pos.centre.y = Convert.ToDouble(parts[3], provider); + } + + try + { + pos.direction.x = Convert.ToDouble(parts[4]); + } + catch (FormatException) + { + pos.direction.x = Convert.ToDouble(parts[4], provider); + } + + try + { + pos.direction.y = Convert.ToDouble(parts[5]); + } + catch (FormatException) + { + pos.direction.y = Convert.ToDouble(parts[5], provider); + } + + } else { + // misformatted data, return 0 filled position + Console.WriteLine("Misformated position"); + } + + return pos; + } } } diff --git a/software/monitor/monitor/MonitorUI.cs b/software/monitor/monitor/MonitorUI.cs index d042c15..0835562 100644 --- a/software/monitor/monitor/MonitorUI.cs +++ b/software/monitor/monitor/MonitorUI.cs @@ -23,6 +23,7 @@ using System; using Gtk; using Gdk; +using Cairo; using monitor; @@ -41,6 +42,11 @@ public partial class MainWindow : Gtk.Window /// private Pixbuf drawingareaCameraPixbuf; + /// + /// Position used for displaying position + /// + private DestijlCommandManager.Position position=new DestijlCommandManager.Position(); + /// /// List of availble state for the application /// @@ -200,8 +206,8 @@ public partial class MainWindow : Gtk.Window a.RetVal = true; } - private byte[] imageComplete; - private byte[] imageInProgress; + //private byte[] imageComplete; + //private byte[] imageInProgress; /// /// Callback called when new message is received from server @@ -209,17 +215,20 @@ public partial class MainWindow : Gtk.Window /// Header of message /// Data of message /// Raw buffer corresponding of received message - public void OnCommandReceivedEvent(string header, string data, byte[] buffer) + public void OnCommandReceivedEvent(string header, string data) { - if (buffer==null) + if (header == null) { // we have lost server ChangeState(SystemState.NotConnected); - MessagePopup(MessageType.Error, + Gtk.Application.Invoke(delegate + { + MessagePopup(MessageType.Error, ButtonsType.Ok, "Server lost", "Server is down: disconnecting"); - cmdManager.Close(); + cmdManager.Close(); + }); } // if we have received a valid message @@ -228,75 +237,104 @@ public partial class MainWindow : Gtk.Window #if DEBUG // print message content if (header.Length > 4) - Console.WriteLine("Bad header(" + buffer.Length + ")"); - else - Console.WriteLine("Received header (" + header.Length + "): " + header); + Console.WriteLine("Bad header(" + header.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) - { - imageComplete = imageInProgress; - imageInProgress = buffer; - } - else - { - if (imageInProgress == null) imageInProgress = buffer; - else - { - Array.Resize(ref imageInProgress, imageInProgress.Length + buffer.Length); - System.Buffer.BlockCopy(buffer, 0, imageInProgress, imageInProgress.Length - buffer.Length, buffer.Length); - } - } + //if (header == DestijlCommandList.CAMERA_IMAGE) + //{ + // imageComplete = imageInProgress; + // //TODO: Decoder le base64 pour recuperer le JPG + // imageInProgress = buffer; + //} + //else + //{ + // if (imageInProgress == null) imageInProgress = buffer; + // else + // { + // Array.Resize(ref imageInProgress, imageInProgress.Length + buffer.Length); + // System.Buffer.BlockCopy(buffer, 0, imageInProgress, imageInProgress.Length - buffer.Length, buffer.Length); + // } + //} // depending on message received (based on header) // launch correponding action - if (header.ToUpper() == DestijlCommandList.HeaderStmBat) + header = header.ToUpper(); + + if (header == DestijlCommandList.ROBOT_BATTERY_LEVEL) { + string batLevel = ""; + switch (data[0]) { case '2': - labelBatteryLevel.Text = "High"; + batLevel = "High"; break; case '1': - labelBatteryLevel.Text = "Low"; + batLevel = "Low"; break; case '0': - labelBatteryLevel.Text = "Empty"; + batLevel = "Empty"; break; default: - labelBatteryLevel.Text = "Invalid value"; + batLevel = "Invalid value"; break; } + + Gtk.Application.Invoke(delegate + { + labelBatteryLevel.Text = batLevel; + }); } - else if (header.ToUpper() == DestijlCommandList.HeaderStmImage) + else if (header == DestijlCommandList.CAMERA_IMAGE) { // 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); + //if (imageComplete != null) + //{ + //TODO: Decoder le base64 et convertir en JPG + byte[] image = Convert.FromBase64String(data); + //byte[] image = new byte[imageComplete.Length - 4]; + //System.Buffer.BlockCopy(imageComplete, 4, image, 0, image.Length); - imageReceivedCounter++; - try + imageReceivedCounter++; + + try + { + drawingareaCameraPixbuf = new Pixbuf(image); + + Gtk.Application.Invoke(delegate { - drawingareaCameraPixbuf = new Pixbuf(image); drawingAreaCamera.QueueDraw(); - } - catch (GLib.GException) - { - badImageReceivedCounter++; -#if DEBUG - Console.WriteLine("Bad Image: " + badImageReceivedCounter + - " / " + imageReceivedCounter + - " (" + badImageReceivedCounter * 100 / imageReceivedCounter + "%)"); -#endif - } + }); } + catch (GLib.GException) + { + badImageReceivedCounter++; +#if DEBUG + Console.WriteLine("Bad Image: " + badImageReceivedCounter + + " / " + imageReceivedCounter + + " (" + badImageReceivedCounter * 100 / imageReceivedCounter + "%)"); +#endif + } + //} + } + else if (header == DestijlCommandList.CAMERA_POSITION) + { + //Console.WriteLine("Pos data: " + data); + + position = DestijlCommandManager.DecodePosition(data); + //Console.WriteLine("decoded position: " + position.ToString()); + + Gtk.Application.Invoke(delegate + { + drawingAreaCamera.QueueDraw(); + }); } } } @@ -417,10 +455,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(); } @@ -557,9 +595,7 @@ public partial class MainWindow : Gtk.Window { if (cmdManager.CameraClose() != DestijlCommandManager.CommandStatus.Success) { - MessagePopup(MessageType.Error, - ButtonsType.Ok, "Error", - "Error when closing camera: bad answer for supervisor or timeout"); + Console.WriteLine("Error when closing camera: bad answer for supervisor or timeout"); } } else // camera is not active, switch it on @@ -569,10 +605,8 @@ public partial class MainWindow : Gtk.Window if (cmdManager.CameraOpen() != DestijlCommandManager.CommandStatus.Success) { - MessagePopup(MessageType.Error, - ButtonsType.Ok, "Error", - "Error when opening camera: bad answer for supervisor or timeout"); - checkButtonCameraOn.Active = false; + Console.WriteLine("Error when opening camera: bad answer for supervisor or timeout"); + //checkButtonCameraOn.Active = false; } } } @@ -589,20 +623,16 @@ public partial class MainWindow : Gtk.Window { if (cmdManager.CameraStopComputePosition() != DestijlCommandManager.CommandStatus.Success) { - MessagePopup(MessageType.Error, - ButtonsType.Ok, "Error", - "Error when stopping position reception: bad answer for supervisor or timeout"); + Console.WriteLine("Error when stopping position reception: bad answer for supervisor or timeout"); } } else // start reception of robot position { if (cmdManager.CameraComputePosition() != DestijlCommandManager.CommandStatus.Success) { - MessagePopup(MessageType.Error, - ButtonsType.Ok, "Error", - "Error when starting getting robot position: bad answer for supervisor or timeout"); + Console.WriteLine("Error when starting getting robot position: bad answer for supervisor or timeout"); - checkButtonRobotPosition.Active = false; + //checkButtonRobotPosition.Active = false; } } } @@ -657,6 +687,47 @@ public partial class MainWindow : Gtk.Window (areaHeight - displayPixbuf.Height) / 2, displayPixbuf.Width, displayPixbuf.Height, RgbDither.Normal, 0, 0); + + if (checkButtonRobotPosition.Active) { + Cairo.Context cr = Gdk.CairoHelper.Create(area.GdkWindow); + Cairo.Color textFontColor = new Cairo.Color(0.8, 0, 0); + + cr.SelectFontFace("Cantarell", FontSlant.Normal, FontWeight.Bold); + cr.SetSourceColor(textFontColor); + cr.SetFontSize(16); + + double space = 0.0; + string text = "Direction (" + position.direction.x.ToString("0.##") + " ; " + position.direction.y.ToString("0.##") +")"; + TextExtents te = cr.TextExtents(text); + cr.MoveTo(areaWidth - te.Width-5, + areaHeight - te.Height -5); + space = te.Height; + cr.ShowText(text); + + text = "Centre (" + position.centre.x.ToString("0.##") + " ; " + position.centre.y.ToString("0.##") + ")"; + te = cr.TextExtents(text); + cr.MoveTo(areaWidth - te.Width - 5, + areaHeight - te.Height - 5 - space-5); + space = space+ te.Height+5; + cr.ShowText(text); + + text = "Angle: " + position.angle.ToString("0.##"); + te = cr.TextExtents(text); + cr.MoveTo(areaWidth - te.Width - 5, + areaHeight - te.Height - 5 - space - 5); + space = space+ te.Height+5; + cr.ShowText(text); + + text = "ID: " + position.robotID; + te = cr.TextExtents(text); + cr.MoveTo(areaWidth - te.Width - 5, + areaHeight - te.Height - 5 - space-5); + + cr.ShowText(text); + + ((IDisposable)cr.GetTarget()).Dispose(); + ((IDisposable)cr).Dispose(); + } } /// diff --git a/software/monitor/monitor/monitor b/software/monitor/monitor/monitor index d65f28c..fe4e274 100755 Binary files a/software/monitor/monitor/monitor and b/software/monitor/monitor/monitor differ diff --git a/software/raspberry/superviseur-robot/.dep.inc b/software/raspberry/superviseur-robot/.dep.inc deleted file mode 100644 index 38ba445..0000000 --- a/software/raspberry/superviseur-robot/.dep.inc +++ /dev/null @@ -1,5 +0,0 @@ -# This code depends on make tool being used -DEPFILES=$(wildcard $(addsuffix .d, ${OBJECTFILES} ${TESTOBJECTFILES})) -ifneq (${DEPFILES},) -include ${DEPFILES} -endif diff --git a/software/raspberry/superviseur-robot/.idea/.name b/software/raspberry/superviseur-robot/.idea/.name deleted file mode 100644 index 02eef87..0000000 --- a/software/raspberry/superviseur-robot/.idea/.name +++ /dev/null @@ -1 +0,0 @@ -ProjDestijl \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/.idea/codeStyles/Project.xml b/software/raspberry/superviseur-robot/.idea/codeStyles/Project.xml deleted file mode 100644 index 30aa626..0000000 --- a/software/raspberry/superviseur-robot/.idea/codeStyles/Project.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/.idea/misc.xml b/software/raspberry/superviseur-robot/.idea/misc.xml deleted file mode 100644 index 8822db8..0000000 --- a/software/raspberry/superviseur-robot/.idea/misc.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/.idea/modules.xml b/software/raspberry/superviseur-robot/.idea/modules.xml deleted file mode 100644 index 7a92ef3..0000000 --- a/software/raspberry/superviseur-robot/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/.idea/superviseur-robot.iml b/software/raspberry/superviseur-robot/.idea/superviseur-robot.iml deleted file mode 100644 index f08604b..0000000 --- a/software/raspberry/superviseur-robot/.idea/superviseur-robot.iml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/.idea/vcs.xml b/software/raspberry/superviseur-robot/.idea/vcs.xml deleted file mode 100644 index c2365ab..0000000 --- a/software/raspberry/superviseur-robot/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/.idea/workspace.xml b/software/raspberry/superviseur-robot/.idea/workspace.xml deleted file mode 100644 index 2579f0f..0000000 --- a/software/raspberry/superviseur-robot/.idea/workspace.xml +++ /dev/null @@ -1,864 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -