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 13KB

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