No Description
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

commonitor.cpp 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329
  1. /*
  2. * Copyright (C) 2018 dimercur
  3. *
  4. * This program is free software: you can redistribute it and/or modify
  5. * it under the terms of the GNU General Public License as published by
  6. * the Free Software Foundation, either version 3 of the License, or
  7. * (at your option) any later version.
  8. *
  9. * This program is distributed in the hope that it will be useful,
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. * GNU General Public License for more details.
  13. *
  14. * You should have received a copy of the GNU General Public License
  15. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  16. */
  17. #include "commonitor.h"
  18. #include <iostream>
  19. #include <sys/socket.h>
  20. #include <netdb.h>
  21. #include <unistd.h>
  22. #include <arpa/inet.h>
  23. #include <netinet/in.h>
  24. #include <algorithm>
  25. #include <stdexcept>
  26. #include <string>
  27. /*
  28. * @brief Constants used for sending commands to monitor
  29. */
  30. const string LABEL_MONITOR_ANSWER_ACK = "AACK";
  31. const string LABEL_MONITOR_ANSWER_NACK = "ANAK";
  32. const string LABEL_MONITOR_ANSWER_COM_ERROR = "ACER";
  33. const string LABEL_MONITOR_ANSWER_TIMEOUT = "ATIM";
  34. const string LABEL_MONITOR_ANSWER_CMD_REJECTED = "ACRJ";
  35. const string LABEL_MONITOR_MESSAGE = "MSSG";
  36. const string LABEL_MONITOR_CAMERA_OPEN = "COPN";
  37. const string LABEL_MONITOR_CAMERA_CLOSE = "CCLS";
  38. const string LABEL_MONITOR_CAMERA_IMAGE = "CIMG";
  39. const string LABEL_MONITOR_CAMERA_ARENA_ASK = "CASA";
  40. const string LABEL_MONITOR_CAMERA_ARENA_INFIRME = "CAIN";
  41. const string LABEL_MONITOR_CAMERA_ARENA_CONFIRM = "CACO";
  42. const string LABEL_MONITOR_CAMERA_POSITION_COMPUTE = "CPCO";
  43. const string LABEL_MONITOR_CAMERA_POSITION_STOP = "CPST";
  44. const string LABEL_MONITOR_CAMERA_POSITION = "CPOS";
  45. const string LABEL_MONITOR_ROBOT_COM_OPEN = "ROPN";
  46. const string LABEL_MONITOR_ROBOT_COM_CLOSE = "RCLS";
  47. const string LABEL_MONITOR_ROBOT_PING = "RPIN";
  48. const string LABEL_MONITOR_ROBOT_RESET = "RRST";
  49. const string LABEL_MONITOR_ROBOT_START_WITHOUT_WD = "RSOW";
  50. const string LABEL_MONITOR_ROBOT_START_WITH_WD = "RSWW";
  51. const string LABEL_MONITOR_ROBOT_RELOAD_WD = "RLDW";
  52. const string LABEL_MONITOR_ROBOT_MOVE = "RMOV";
  53. const string LABEL_MONITOR_ROBOT_TURN = "RTRN";
  54. const string LABEL_MONITOR_ROBOT_GO_FORWARD = "RGFW";
  55. const string LABEL_MONITOR_ROBOT_GO_BACKWARD = "RGBW";
  56. const string LABEL_MONITOR_ROBOT_GO_LEFT = "RGLF";
  57. const string LABEL_MONITOR_ROBOT_GO_RIGHT = "RGRI";
  58. const string LABEL_MONITOR_ROBOT_STOP = "RSTP";
  59. const string LABEL_MONITOR_ROBOT_POWEROFF = "RPOF";
  60. const string LABEL_MONITOR_ROBOT_BATTERY_LEVEL = "RBLV";
  61. const string LABEL_MONITOR_ROBOT_GET_BATTERY = "RGBT";
  62. const string LABEL_MONITOR_ROBOT_GET_STATE = "RGST";
  63. const string LABEL_MONITOR_ROBOT_CURRENT_STATE = "RCST";
  64. const string LABEL_SEPARATOR_CHAR = ":";
  65. /**
  66. * Create a server and open a socket over TCP
  67. *
  68. * @param port Port used for communication
  69. * @return Socket number
  70. * @throw std::runtime_error if it fails
  71. */
  72. int ComMonitor::Open(int port) {
  73. struct sockaddr_in server;
  74. socketFD = socket(AF_INET, SOCK_STREAM, 0);
  75. if (socketFD < 0) {
  76. throw std::runtime_error{"Can not create socket"};
  77. }
  78. server.sin_addr.s_addr = INADDR_ANY;
  79. server.sin_family = AF_INET;
  80. server.sin_port = htons(port);
  81. if (bind(socketFD, (struct sockaddr *) &server, sizeof (server)) < 0) {
  82. cerr<<"["<<__PRETTY_FUNCTION__<<"] Can not bind socket ("<<to_string(port)<<")"<<endl<<flush;
  83. throw std::runtime_error{"Can not bind socket"};
  84. }
  85. listen(socketFD, 1);
  86. return socketFD;
  87. }
  88. /**
  89. * Close socket and server
  90. */
  91. void ComMonitor::Close() {
  92. close(socketFD);
  93. socketFD = -1;
  94. }
  95. /**
  96. * Wait for a client to connect
  97. * @return Client number
  98. * @throw std::runtime_error if it fails
  99. */
  100. int ComMonitor::AcceptClient() {
  101. struct sockaddr_in client;
  102. int c = sizeof (struct sockaddr_in);
  103. clientID = accept(socketFD, (struct sockaddr *) &client, (socklen_t*) & c);
  104. if (clientID < 0)
  105. throw std::runtime_error {"Accept failed"};
  106. return clientID;
  107. }
  108. /**
  109. * Send a message to monitor
  110. *
  111. * @param msg Message to send to monitor
  112. * @attention Message given in parameter will be destroyed (delete) after being sent. No need for user to delete message after that.
  113. * @warning Write is not thread safe : check that multiple tasks can't access this method simultaneously
  114. */
  115. void ComMonitor::Write(Message &msg) {
  116. string str;
  117. // Call user method before Write
  118. Write_Pre();
  119. /* Convert message to string to send to monitor */
  120. str = MessageToString(msg);
  121. //cout << "Message sent to monitor: " << str->c_str() << endl;
  122. write(clientID, str.c_str(), str.length());
  123. delete(&msg);
  124. // Call user method after write
  125. Write_Post();
  126. }
  127. /**
  128. * Receive a message from monitor
  129. *
  130. * @return Message received from monitor
  131. * @attention Message provided is produced by the method. You must delete it when you are done using it
  132. * @warning Read is not thread safe : check that multiple tasks can't access this method simultaneously
  133. */
  134. Message *ComMonitor::Read() {
  135. char length = 0;
  136. string s;
  137. char data;
  138. bool endReception = false;
  139. Message *msg;
  140. // Call user method before read
  141. Read_Pre();
  142. if (clientID > 0) {
  143. while (!endReception) {
  144. if ((length = recv(clientID, (void*) &data, 1, MSG_WAITALL)) > 0) {
  145. if (data != '\n') {
  146. s += data;
  147. } else endReception = true;
  148. }
  149. }
  150. if (length <= 0) msg = new Message(MESSAGE_MONITOR_LOST);
  151. else {
  152. msg = StringToMessage(s);
  153. }
  154. }
  155. // Call user method after read
  156. Read_Post();
  157. return msg;
  158. }
  159. /**
  160. * Method used internally to convert a message content to a string that can be sent over TCP
  161. * @param msg Message to be converted
  162. * @return A string, image of the message
  163. */
  164. string ComMonitor::MessageToString(Message &msg) {
  165. int id;
  166. string str;
  167. Message *localMsg = &msg;
  168. Position pos;
  169. id = msg.GetID();
  170. switch (id) {
  171. case MESSAGE_ANSWER_ACK :
  172. str.append(LABEL_MONITOR_ANSWER_ACK);
  173. break;
  174. case MESSAGE_ANSWER_NACK:
  175. str.append(LABEL_MONITOR_ANSWER_NACK);
  176. break;
  177. case MESSAGE_ANSWER_ROBOT_TIMEOUT:
  178. str.append(LABEL_MONITOR_ANSWER_TIMEOUT);
  179. break;
  180. case MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND:
  181. str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
  182. break;
  183. case MESSAGE_ANSWER_ROBOT_ERROR:
  184. str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
  185. break;
  186. case MESSAGE_ANSWER_COM_ERROR:
  187. str.append(LABEL_MONITOR_ANSWER_COM_ERROR);
  188. break;
  189. case MESSAGE_CAM_POSITION:
  190. pos = ((MessagePosition*) & msg)->GetPosition();
  191. str.append(LABEL_MONITOR_CAMERA_POSITION + LABEL_SEPARATOR_CHAR + to_string(pos.robotId) + ";" +
  192. to_string(pos.angle) + ";" + to_string(pos.center.x) + ";" + to_string(pos.center.y) + ";" +
  193. to_string(pos.direction.x) + ";" + to_string(pos.direction.y));
  194. break;
  195. case MESSAGE_CAM_IMAGE:
  196. str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) & msg)->GetImage()->ToBase64());
  197. break;
  198. case MESSAGE_ROBOT_BATTERY_LEVEL:
  199. str.append(LABEL_MONITOR_ROBOT_BATTERY_LEVEL + LABEL_SEPARATOR_CHAR + to_string(((MessageBattery*) & msg)->GetLevel()));
  200. break;
  201. case MESSAGE_ROBOT_STATE_BUSY:
  202. str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "1");
  203. break;
  204. case MESSAGE_ROBOT_STATE_NOT_BUSY:
  205. str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "0");
  206. break;
  207. case MESSAGE_LOG:
  208. str.append(LABEL_MONITOR_MESSAGE + LABEL_SEPARATOR_CHAR + ((MessageString*) & msg)->GetString());
  209. break;
  210. case MESSAGE_EMPTY:
  211. str.append(""); //empty string
  212. break;
  213. default:
  214. cerr<<"["<<__PRETTY_FUNCTION__<<"] (from ComMonitor::Write): Invalid message to send ("<<msg.ToString()<<")"<<endl<<flush;
  215. throw std::runtime_error {"Invalid message to send"};
  216. }
  217. str.append("\n");
  218. return str;
  219. }
  220. /**
  221. * Method used internally to convert a string received over TCP to a message
  222. * @param s String containing message
  223. * @return A message, image of the string
  224. */
  225. Message *ComMonitor::StringToMessage(string &s) {
  226. Message *msg;
  227. size_t pos;
  228. string org = s;
  229. string tokenCmd;
  230. string tokenData;
  231. /* Separate command from data if string contains a ':' */
  232. if ((pos = org.find(LABEL_SEPARATOR_CHAR)) != string::npos) {
  233. tokenCmd = org.substr(0, pos);
  234. org.erase(0, pos + 1);
  235. tokenData = org;
  236. } else tokenCmd = org;
  237. /* Convert command to message */
  238. if (tokenCmd.find(LABEL_MONITOR_ROBOT_MOVE) != string::npos) {
  239. msg = new MessageInt(MESSAGE_ROBOT_MOVE, stoi(tokenData));
  240. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_TURN) != string::npos) {
  241. msg = new MessageInt(MESSAGE_ROBOT_TURN, stoi(tokenData));
  242. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITHOUT_WD) != string::npos) {
  243. msg = new Message(MESSAGE_ROBOT_START_WITHOUT_WD);
  244. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITH_WD) != string::npos) {
  245. msg = new Message(MESSAGE_ROBOT_START_WITH_WD);
  246. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RELOAD_WD) != string::npos) {
  247. msg = new Message(MESSAGE_ROBOT_RELOAD_WD);
  248. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_PING) != string::npos) {
  249. msg = new Message(MESSAGE_ROBOT_PING);
  250. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RESET) != string::npos) {
  251. msg = new Message(MESSAGE_ROBOT_RESET);
  252. } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_ASK) != string::npos) {
  253. msg = new Message(MESSAGE_CAM_ASK_ARENA);
  254. } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_CONFIRM) != string::npos) {
  255. msg = new Message(MESSAGE_CAM_ARENA_CONFIRM);
  256. } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_INFIRME) != string::npos) {
  257. msg = new Message(MESSAGE_CAM_ARENA_INFIRM);
  258. } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_CLOSE) != string::npos) {
  259. msg = new Message(MESSAGE_CAM_CLOSE);
  260. } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_OPEN) != string::npos) {
  261. msg = new Message(MESSAGE_CAM_OPEN);
  262. } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_COMPUTE) != string::npos) {
  263. msg = new Message(MESSAGE_CAM_POSITION_COMPUTE_START);
  264. } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_STOP) != string::npos) {
  265. msg = new Message(MESSAGE_CAM_POSITION_COMPUTE_STOP);
  266. } else if (tokenCmd.find(LABEL_MONITOR_MESSAGE) != string::npos) {
  267. msg = new MessageString(MESSAGE_LOG, tokenData);
  268. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_CLOSE) != string::npos) {
  269. msg = new Message(MESSAGE_ROBOT_COM_CLOSE);
  270. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_OPEN) != string::npos) {
  271. msg = new Message(MESSAGE_ROBOT_COM_OPEN);
  272. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_BATTERY) != string::npos) {
  273. msg = new Message(MESSAGE_ROBOT_BATTERY_GET);
  274. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_STATE) != string::npos) {
  275. msg = new Message(MESSAGE_ROBOT_STATE_GET);
  276. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_FORWARD) != string::npos) {
  277. msg = new Message(MESSAGE_ROBOT_GO_FORWARD);
  278. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_BACKWARD) != string::npos) {
  279. msg = new Message(MESSAGE_ROBOT_GO_BACKWARD);
  280. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_LEFT) != string::npos) {
  281. msg = new Message(MESSAGE_ROBOT_GO_LEFT);
  282. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_RIGHT) != string::npos) {
  283. msg = new Message(MESSAGE_ROBOT_GO_RIGHT);
  284. } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_POWEROFF) != string::npos) {
  285. msg = new Message(MESSAGE_ROBOT_POWEROFF);
  286. } else {
  287. msg = new Message(MESSAGE_EMPTY);
  288. }
  289. return msg;
  290. }