123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675 |
- //
- // DestijlCommandManager.cs
- //
- // Author:
- // Di MERCURIO Sébastien <dimercur@insa-toulouse.fr>
- //
- // Copyright (c) 2018 INSA - DGEI
- //
- // This program is free software: you can redistribute it and/or modify
- // it under the terms of the GNU General Public License as published by
- // the Free Software Foundation, either version 3 of the License, or
- // (at your option) any later version.
- //
- // This program is distributed in the hope that it will be useful,
- // but WITHOUT ANY WARRANTY; without even the implied warranty of
- // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- // GNU General Public License for more details.
- //
- // You should have received a copy of the GNU General Public License
- // along with this program. If not, see <http://www.gnu.org/licenses/>.
-
- using System;
- using System.Globalization;
-
- namespace monitor
- {
- /// <summary>
- /// Commands and options parameters used in Destijl project when communicating with server
- /// </summary>
- public static class DestijlCommandList
- {
- 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 char SEPARATOR_CHAR = ':';
-
- //public const string HeaderMtsComDmb = "COM";
- //public const string HeaderMtsDmbOrder = "DMB";
- //public const string HeaderMtsCamera = "CAM";
- //public const string HeaderMtsMessage = "MSG";
-
- //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";
- }
-
- /// <summary>
- /// Commands used for robot messages
- /// </summary>
- //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";
- //}
-
- /// <summary>
- /// Specialization class for command manager, which implemnent destijl protocol between monitor and server
- /// </summary>
- public class DestijlCommandManager
- {
- /// <summary>
- /// Command Manager object
- /// </summary>
- private CommandManager commandManager = null;
-
- /// <summary>
- /// Part of received message corresponding to command header
- /// </summary>
- private string receivedHeader = null;
-
- /// <summary>
- /// Part of received message corresponding to command data
- /// </summary>
- private string receivedData = null;
-
- /// <summary>
- /// Callback for sending received data to application level
- /// </summary>
- public delegate void CommandReceivedEvent(string header, string data);
- public CommandReceivedEvent commandReceivedEvent = null;
-
- /// <summary>
- /// Timeout used for command with acknowledge
- /// </summary>
- public double timeout = 100;
-
- /// <summary>
- /// List of available return status
- /// </summary>
- public enum CommandStatus
- {
- Success,
- Rejected,
- InvalidAnswer,
- Busy,
- CommunicationLostWithRobot,
- 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;
- }
- }
-
- /// <summary>
- /// Initializes a new instance of the <see cref="monitor.DestijlCommandManager"/> class.
- /// </summary>
- /// <param name="callback">Callback reference for reception of data</param>
- public DestijlCommandManager(CommandReceivedEvent callback)
- {
- commandManager = new CommandManager(OnCommandReceived);
- this.commandReceivedEvent += callback;
- }
-
- /// <summary>
- /// Releases unmanaged resources and performs other cleanup operations before the
- /// <see cref="monitor.DestijlCommandManager"/> is reclaimed by garbage collection.
- /// </summary>
- ~DestijlCommandManager()
- {
- if (commandManager != null) commandManager.Close();
- }
-
- /// <summary>
- /// Callback used for receiving data from lower layer (CommandManager class)
- /// </summary>
- /// <param name="msg">String containing received message</param>
- /// <param name="buffer">Raw buffer to be used when data are not in ascii format (image for example)</param>
- private void OnCommandReceived(string msg)
- {
- // 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];
- else receivedHeader = null;
-
- // if msgs array contains at least two elements, second element is normally data
- if (msgs.Length >= 2) receivedData = msgs[1];
- else receivedData = null;
-
- // when split is done, provide data to application
- this.commandReceivedEvent?.Invoke(receivedHeader, receivedData);
- }
-
- /// <summary>
- /// Open the specified hostname server, using default port number.
- /// </summary>
- /// <returns>true if connection succeded, false otherwise</returns>
- /// <param name="hostname">Hostname to connect to</param>
- public bool Open(string hostname)
- {
- return this.Open(hostname, Client.defaultPort);
- }
-
- /// <summary>
- /// Open connection to server "host", with port number "port"
- /// </summary>
- /// <returns>true if connection succeded, false otherwise</returns>
- /// <param name="hostname">Hostname to connect to</param>
- /// <param name="port">Port number for connection</param>
- public bool Open(string hostname, int port)
- {
- if (commandManager != null) return commandManager.Open(hostname, port);
- else return false;
- }
-
- /// <summary>
- /// Close connection to server
- /// </summary>
- public void Close()
- {
- if (commandManager != null) commandManager.Close();
- }
-
- /// <summary>
- /// Creates the command to send to server, based on header and data provided
- /// </summary>
- /// <returns>The command string</returns>
- /// <param name="header">Header part of the command</param>
- /// <param name="data">Data part of the command</param>
- private string CreateCommand(string header, string data)
- {
- return header + DestijlCommandList.SEPARATOR_CHAR + data+"\n";
- }
-
- /// <summary>
- /// Creates the command to send to server, based on header
- /// </summary>
- /// <returns>The command string</returns>
- /// <param name="header">Header part of the command</param>
- private string CreateCommand(string header)
- {
- return header + DestijlCommandList.SEPARATOR_CHAR+"\n";
- }
-
- /// <summary>
- /// Provide DestijlCommandManager.CommandStatus based on status received by CommandManager.SendCommand and answer string
- /// </summary>
- /// <returns>Status compatible with DestijlCommandManager.CommandStatus type</returns>
- /// <param name="localStatus">Status provided by CommandManager.SendCommand</param>
- /// <param name="answer">Answer provided by CommandManager.SendCommand</param>
- private CommandStatus DecodeStatus(CommandManager.CommandManagerStatus localStatus, string answer)
- {
- CommandStatus status = CommandStatus.Success;
-
- // if timeout occures, return CommandStatus.CommunicationLostWithServer
- if (localStatus == CommandManager.CommandManagerStatus.Timeout) status = CommandStatus.CommunicationLostWithServer;
- // if a command is currently processed, return Busy
- else if (localStatus == CommandManager.CommandManagerStatus.Busy) status = CommandStatus.Busy;
- else
- {
- if (answer != null)
- {
- // if command is not acknowledged, return 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.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;
- }
- }
-
- return status;
- }
-
- /// <summary>
- /// Open communication with robot and wait acknowledge
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus RobotOpenCom()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_COM_OPEN),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Close communication with robot and wait acknowledge
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus RobotCloseCom()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_COM_CLOSE),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Ping the robot.
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus RobotPing()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_PING),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Reset robot and let it in idle mode
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus RobotReset()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_RESET),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Start robot, enabling watchdog
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus RobotStartWithWatchdog()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_START_WITH_WD),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Start robot, without enabling watchdog
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus RobotStartWithoutWatchdog()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_START_WITHOUT_WD),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Move robot forward or backward, for a distance expressed in millimeter
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- /// <param name="distance">Distance of mouvment, in millimeter</param>
- public CommandStatus RobotMove(int distance)
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_MOVE, Convert.ToString(distance)),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Make robot turn left or right, for a given angle
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- /// <param name="angle">Angle of turn, in degree (negative for left, positive for right)</param>
- public CommandStatus RobotTurn(int angle)
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_TURN, Convert.ToString(angle)),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Request robot battery level
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus RobotGetBattery()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_GET_BATTERY),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Power off robot
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus RobotPowerOff()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.ROBOT_POWEROFF),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Open camera on remote device
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus CameraOpen()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.CAMERA_OPEN),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Close camera on remote device
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus CameraClose()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.CAMERA_CLOSE),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Request still image of detected arena
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus CameraAskArena()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.CAMERA_ARENA_ASK),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Confirm arena detection (after requesting image of detected arena, using CameraAskArena
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus CameraArenaConfirm()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.CAMERA_ARENA_CONFIRM),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Reject arena detected (after requesting image of detected arena, using CameraAskArena
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus CameraArenaInfirm()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.CAMERA_ARENA_INFIRM),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Request robot position computing
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus CameraComputePosition()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.CAMERA_POSITION_COMPUTE),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- /// <summary>
- /// Stop robot position computing
- /// </summary>
- /// <returns>Command status (see DecodeStatus)</returns>
- public CommandStatus CameraStopComputePosition()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- 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;
- }
- }
- }
|