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.

messages.cpp 13KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533
  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 "messages.h"
  18. #include <exception>
  19. #include <stdexcept>
  20. #include <string>
  21. /*
  22. * @brief Constants used with ToString method, for printing message id
  23. */
  24. const string MESSAGE_ID_STRING[] = {
  25. "Empty",
  26. "Log",
  27. "Answer [Acknowledge]",
  28. "Answer [Not Acknowledge]",
  29. "Answer [Command timeout]",
  30. "Answer [Command unknown]",
  31. "Answer [Command error]",
  32. "Answer [Communication error]",
  33. "Monitor connection lost",
  34. "Open serial com",
  35. "Close serial com",
  36. "Open camera",
  37. "Close camera",
  38. "Ask for arena",
  39. "Arena confirmed",
  40. "Arena infirmed",
  41. "Compute position",
  42. "Stop compute position",
  43. "Position",
  44. "Image",
  45. "Robot ping",
  46. "Robot reset",
  47. "Robot start with watchdog",
  48. "Robot start without watchdog",
  49. "Robot reload watchdog",
  50. "Robot move",
  51. "Robot turn",
  52. "Robot go forward",
  53. "Robot go backward",
  54. "Robot go left",
  55. "Robot go right",
  56. "Robot stop",
  57. "Robot poweroff",
  58. "Robot get battery",
  59. "Robot battery level",
  60. "Robot get state",
  61. "Robot current state",
  62. "Robot state [Not busy]",
  63. "Robot state [Busy]"
  64. };
  65. /*
  66. * @brief Constants used with ToString method, for printing answer id
  67. */
  68. const string ANSWER_ID_STRING[] = {
  69. "Acknowledge",
  70. "Not Acknowledge",
  71. "Robot lost",
  72. "Timeout error",
  73. "Unknown command",
  74. "Invalid or refused command",
  75. "Checksum error"
  76. };
  77. /**
  78. * Create a new, empty message
  79. */
  80. Message::Message() {
  81. this->messageID = MESSAGE_EMPTY;
  82. }
  83. /**
  84. * Create a new, empty message
  85. */
  86. Message::Message(MessageID id) {
  87. SetID(id);
  88. }
  89. /**
  90. * Destroy message
  91. */
  92. Message::~Message() {
  93. }
  94. /**
  95. * Set message ID
  96. * @param id Message ID
  97. */
  98. void Message::SetID(MessageID id) {
  99. if (CheckID(id)) {
  100. this->messageID = id;
  101. } else throw std::runtime_error {"Invalid message id for Message"};
  102. }
  103. /**
  104. * Translate content of message into a string that can be displayed
  105. * @return A string describing message contents
  106. */
  107. string Message::ToString() {
  108. if (CheckID(this->messageID))
  109. return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"";
  110. else
  111. return "Invalid message";
  112. }
  113. /**
  114. * Allocate a new mesage and copy contents of current message
  115. * @return A message, copy of current
  116. */
  117. Message* Message::Copy() {
  118. Message *msg = new Message(this->messageID);
  119. return msg;
  120. }
  121. /**
  122. * Get message ID
  123. * @return Current message ID
  124. */
  125. bool Message::CheckID(MessageID id) {
  126. if ((id == MESSAGE_CAM_IMAGE) ||
  127. (id == MESSAGE_CAM_POSITION) ||
  128. (id == MESSAGE_ROBOT_MOVE) ||
  129. (id == MESSAGE_ROBOT_TURN) ||
  130. (id == MESSAGE_LOG) ||
  131. (id == MESSAGE_ROBOT_BATTERY_LEVEL)) {
  132. return false;
  133. } else return true;
  134. }
  135. /* MessageInt */
  136. /**
  137. * Create a new, empty int message
  138. */
  139. MessageInt::MessageInt() {
  140. value = 0.0;
  141. }
  142. /**
  143. * Create a new int message, with given ID and value
  144. * @param id Message ID
  145. * @param val Message value
  146. * @throw std::runtime_error if message ID is incompatible with int data
  147. */
  148. MessageInt::MessageInt(MessageID id, int val) {
  149. MessageInt::SetID(id);
  150. value = val;
  151. }
  152. /**
  153. * Set message ID
  154. * @param id Message ID
  155. * @throw std::runtime_error if message ID is incompatible with float data
  156. */
  157. void MessageInt::SetID(MessageID id) {
  158. if (CheckID(id))
  159. messageID = id;
  160. else
  161. throw std::runtime_error {
  162. "Invalid message id for MessageInt"
  163. };
  164. }
  165. /**
  166. * Translate content of message into a string that can be displayed
  167. * @return A string describing message contents
  168. */
  169. string MessageInt::ToString() {
  170. if (CheckID(this->messageID))
  171. return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nValue: " + to_string(this->value);
  172. else
  173. return "Invalid message";
  174. }
  175. /**
  176. * Allocate a new message and copy contents of current message
  177. * @return A message, copy of current
  178. */
  179. Message* MessageInt::Copy() {
  180. return new MessageInt(this->messageID, this->value);
  181. }
  182. /**
  183. * Verify if message ID is compatible with current message type
  184. * @param id Message ID
  185. * @return true, if message ID is acceptable, false otherwise
  186. */
  187. bool MessageInt::CheckID(MessageID id) {
  188. if ((id != MESSAGE_ROBOT_TURN) &&
  189. (id != MESSAGE_ROBOT_MOVE)) {
  190. return false;
  191. } else return true;
  192. }
  193. /* class MessageString */
  194. /**
  195. * Create a new, empty string message
  196. */
  197. MessageString::MessageString() {
  198. s = string("");
  199. }
  200. /**
  201. * Create a new string message, with given ID and string
  202. * @param id Message ID
  203. * @param s Message string
  204. * @throw std::runtime_error if message ID is incompatible with string data
  205. */
  206. MessageString::MessageString(MessageID id, string s) {
  207. MessageString::SetID(id);
  208. this->s = s;
  209. }
  210. /**
  211. * Set message ID
  212. * @param id Message ID
  213. * @throw std::runtime_error if message ID is incompatible with string data
  214. */
  215. void MessageString::SetID(MessageID id) {
  216. if (CheckID(id))
  217. messageID = id;
  218. else
  219. throw std::runtime_error {
  220. "Invalid message id for MessageString"
  221. };
  222. }
  223. /**
  224. * Translate content of message into a string that can be displayed
  225. * @return A string describing message contents
  226. */
  227. string MessageString::ToString() {
  228. if (CheckID(this->messageID))
  229. return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nString: \"" + this->s + "\"";
  230. else
  231. return "Invalid message";
  232. }
  233. /**
  234. * Allocate a new message and copy contents of current message
  235. * @return A message, copy of current
  236. */
  237. Message* MessageString::Copy() {
  238. return new MessageString(this->messageID, this->s);
  239. }
  240. /**
  241. * Verify if message ID is compatible with current message type
  242. * @param id Message ID
  243. * @return true, if message ID is acceptable, false otherwise
  244. */
  245. bool MessageString::CheckID(MessageID id) {
  246. if ((id != MESSAGE_LOG)) {
  247. return false;
  248. } else return true;
  249. }
  250. /* class MessageImg */
  251. /**
  252. * Create a new, empty image message
  253. */
  254. MessageImg::MessageImg() {
  255. image = NULL;
  256. }
  257. /**
  258. * Create a new image message, with given ID and image
  259. * @param id Message ID
  260. * @param image Image
  261. * @throw std::runtime_error if message ID is incompatible with image
  262. */
  263. MessageImg::MessageImg(MessageID id, Img* image) {
  264. MessageImg::SetID(id);
  265. MessageImg::SetImage(image);
  266. }
  267. /**
  268. * Destroy Image message
  269. */
  270. MessageImg::~MessageImg() {
  271. delete (this->image);
  272. }
  273. /**
  274. * Set message ID
  275. * @param id Message ID
  276. * @throw std::runtime_error if message ID is incompatible with image
  277. */
  278. void MessageImg::SetID(MessageID id) {
  279. if (CheckID(id))
  280. messageID = id;
  281. else
  282. throw std::runtime_error {
  283. "Invalid message id for MessageImg"
  284. };
  285. }
  286. /**
  287. * Set message image
  288. * @param image Reference to image object
  289. */
  290. void MessageImg::SetImage(Img* image) {
  291. this->image = image->Copy();
  292. }
  293. /**
  294. * Translate content of message into a string that can be displayed
  295. * @return A string describing message contents
  296. */
  297. string MessageImg::ToString() {
  298. if (CheckID(this->messageID))
  299. return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\n" + this->image->ToString();
  300. else
  301. return "Invalid message";
  302. }
  303. /**
  304. * Allocate a new message and copy contents of current message
  305. * @return A message, copy of current
  306. */
  307. Message* MessageImg::Copy() {
  308. return new MessageImg(this->messageID, this->image->Copy());
  309. }
  310. /**
  311. * Verify if message ID is compatible with current message type
  312. * @param id Message ID
  313. * @return true, if message ID is acceptable, false otherwise
  314. */
  315. bool MessageImg::CheckID(MessageID id) {
  316. if (id != MESSAGE_CAM_IMAGE) {
  317. return false;
  318. } else return true;
  319. }
  320. /* class MessageBattery */
  321. /**
  322. * Create a new, empty battery message
  323. */
  324. MessageBattery::MessageBattery() {
  325. this->level = BATTERY_UNKNOWN;
  326. }
  327. /**
  328. * Create a new battery message, with given ID and battery level
  329. * @param id Message ID
  330. * @param level Battery level
  331. * @throw std::runtime_error if message ID is incompatible with battery
  332. */
  333. MessageBattery::MessageBattery(MessageID id, BatteryLevel level) {
  334. MessageBattery::SetID(id);
  335. MessageBattery::SetLevel(level);
  336. }
  337. /**
  338. * Set message ID
  339. * @param id Message ID
  340. * @throw std::runtime_error if message ID is incompatible with battery
  341. */
  342. void MessageBattery::SetID(MessageID id) {
  343. if (CheckID(id))
  344. messageID = id;
  345. else
  346. throw std::runtime_error {
  347. "Invalid message id for MessageBattery"
  348. };
  349. }
  350. /**
  351. * Set battery level
  352. * @param level Battery level
  353. */
  354. void MessageBattery::SetLevel(BatteryLevel level) {
  355. if ((level < BATTERY_UNKNOWN) || (level > BATTERY_FULL)) {
  356. throw std::runtime_error{
  357. "Invalid battery level for MessageBattery"};
  358. } else {
  359. this->level = level;
  360. }
  361. }
  362. /**
  363. * Translate content of message into a string that can be displayed
  364. * @return A string describing message contents
  365. */
  366. string MessageBattery::ToString() {
  367. string levelString;
  368. switch (this->level) {
  369. case BATTERY_UNKNOWN:
  370. levelString="Unknown";
  371. break;
  372. case BATTERY_EMPTY:
  373. levelString="Empty";
  374. break;
  375. case BATTERY_LOW:
  376. levelString="Low";
  377. break;
  378. case BATTERY_FULL:
  379. levelString="Full";
  380. break;
  381. default:
  382. levelString="Invalid";
  383. }
  384. if (CheckID(this->messageID))
  385. return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nBattery level: \"" + levelString + "\"";
  386. else
  387. return "Invalid message";
  388. }
  389. /**
  390. * Allocate a new message and copy contents of current message
  391. * @return A message, copy of current
  392. */
  393. Message* MessageBattery::Copy() {
  394. return new MessageBattery(this->messageID, this->level);
  395. }
  396. /**
  397. * Verify if message ID is compatible with current message type
  398. * @param id Message ID
  399. * @return true, if message ID is acceptable, false otherwise
  400. */
  401. bool MessageBattery::CheckID(MessageID id) {
  402. if ((id != MESSAGE_ROBOT_BATTERY_LEVEL)) {
  403. return false;
  404. } else return true;
  405. }
  406. /* class MessagePosition */
  407. /**
  408. * Create a new, empty string message
  409. */
  410. MessagePosition::MessagePosition() {
  411. this->pos.angle = 0.0;
  412. this->pos.robotId = 0;
  413. this->pos.center.x=0.0;
  414. this->pos.center.y=0.0;
  415. this->pos.direction.x=0.0;
  416. this->pos.direction.y=0.0;
  417. }
  418. /**
  419. * Create a new string message, with given ID and string
  420. * @param id Message ID
  421. * @param s Message string
  422. * @throw std::runtime_error if message ID is incompatible with string data
  423. */
  424. MessagePosition::MessagePosition(MessageID id, Position& pos) {
  425. MessagePosition::SetID(id);
  426. MessagePosition::SetPosition(pos);
  427. }
  428. /**
  429. * Set message ID
  430. * @param id Message ID
  431. * @throw std::runtime_error if message ID is incompatible with string data
  432. */
  433. void MessagePosition::SetID(MessageID id) {
  434. if (CheckID(id))
  435. messageID = id;
  436. else
  437. throw std::runtime_error {
  438. "Invalid message id for MessagePosition"
  439. };
  440. }
  441. /**
  442. * Set position
  443. * @param pos Reference to position
  444. */
  445. void MessagePosition::SetPosition(Position& pos) {
  446. this->pos.angle = pos.angle;
  447. this->pos.robotId = pos.robotId;
  448. this->pos.center = pos.center;
  449. this->pos.direction = pos.direction;
  450. }
  451. /**
  452. * Translate content of message into a string that can be displayed
  453. * @return A string describing message contents
  454. */
  455. string MessagePosition::ToString() {
  456. if (CheckID(this->messageID))
  457. return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nPosition: \"" + to_string(this->pos.center.x) + ";" + to_string(this->pos.center.y) + "\"";
  458. else
  459. return "Invalid message";
  460. }
  461. /**
  462. * Allocate a new message and copy contents of current message
  463. * @return A message, copy of current
  464. */
  465. Message* MessagePosition::Copy() {
  466. return new MessagePosition(this->messageID, this->pos);
  467. }
  468. /**
  469. * Verify if message ID is compatible with current message type
  470. * @param id Message ID
  471. * @return true, if message ID is acceptable, false otherwise
  472. */
  473. bool MessagePosition::CheckID(MessageID id) {
  474. if ((id != MESSAGE_CAM_POSITION)) {
  475. return false;
  476. } else return true;
  477. }