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.

main.cpp 7.0KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270
  1. /*
  2. * To change this license header, choose License Headers in Project Properties.
  3. * To change this template file, choose Tools | Templates
  4. * and open the template in the editor.
  5. */
  6. /*
  7. * File: main.cpp
  8. * Author: dimercur
  9. *
  10. * Created on 6 novembre 2018, 10:54
  11. */
  12. #include <cstdlib>
  13. #include "image.h"
  14. #include "server.h"
  15. #include "robot.h"
  16. #include "message.h"
  17. #include <iostream>
  18. #include <string>
  19. #include <thread>
  20. #include "definitions.h"
  21. #define HEADER_STM_IMAGE "IMG" // Envoi d'une image
  22. #define HEADER_STM_BAT "BAT" // Envoi de l'état de la batterie
  23. #define HEADER_STM_POS "POS" // Envoi de la position
  24. #define HEADER_STM_NO_ACK "NAK" // Acquittement d'un échec
  25. #define HEADER_STM_ACK "ACK" // Acquittement d'un succès
  26. #define HEADER_STM_MES "MSG" // Message textuel
  27. #define HEADER_STM_LOST_DMB "LCD" // Perte de la communication avec le robot
  28. #define HEADER_MTS_MSG "MSG" // Message directe pour Console Dumber
  29. #define HEADER_MTS_DMB_ORDER "DMB" // Message d'ordre pour le robot
  30. #define HEADER_MTS_COM_DMB "COM" // Message de gestion de la communication avec le robot
  31. #define HEADER_MTS_CAMERA "CAM" // Message de gestion de la camera
  32. #define HEADER_MTS_STOP "STO" // Message d'arrêt du system
  33. int socketID;
  34. char data[1000];
  35. int receivedLength;
  36. bool disconnected = true;
  37. bool dataReady = false;
  38. bool sysTick = false;
  39. bool sendImage = false;
  40. bool sendPos = false;
  41. Image monImage;
  42. Jpg imageCompressed;
  43. pthread_t thread;
  44. typedef struct {
  45. char header[4];
  46. char data[500];
  47. } MessageFromMon;
  48. MessageFromMon *message;
  49. MessageToMon messageAnswered;
  50. std::thread *threadTimer;
  51. std::thread *threadServer;
  52. using namespace std;
  53. /*
  54. *
  55. */
  56. void ThreadServer(void) {
  57. // Recuperation d'une evenutelle commande sur le serveur
  58. while (1) {
  59. receivedLength = receiveDataFromServer(data, 1000);
  60. if (receivedLength > 0) dataReady = true;
  61. }
  62. }
  63. void ThreadTimer(void) {
  64. while (1) {
  65. //std::this_thread::sleep_for(std::chrono::seconds )
  66. sleep(1);
  67. sysTick = true;
  68. }
  69. }
  70. void printReceivedMessage(MessageFromMon *mes) {
  71. cout << "Received " + to_string(receivedLength) + " data\n";
  72. cout << "Header: ";
  73. for (int i = 0; i < 4; i++) {
  74. cout << mes->header[i];
  75. }
  76. cout << "\nData: ";
  77. for (int i = 0; i < receivedLength - 4; i++) {
  78. cout << mes->data[i];
  79. }
  80. cout << "\n";
  81. }
  82. int sendAnswer(string cmd, string data) {
  83. int status = 0;
  84. string msg;
  85. msg = cmd + ':' + data;
  86. cout << "Answer: " + msg;
  87. cout << "\n";
  88. sendDataToServer((char*) msg.c_str(), msg.length());
  89. return status;
  90. }
  91. int sendBinaryData(string cmd, unsigned char* data) {
  92. int status = 0;
  93. string msg;
  94. //TODO: Faire une version binaire de sendDataToServer
  95. //TODO: Gerer la concatenation du header et des données binaires
  96. msg = cmd + ':' + data;
  97. cout << "Answer: " + msg;
  98. cout << "\n";
  99. sendDataToServer((char*) msg.c_str(), msg.length());
  100. return status;
  101. }
  102. int decodeMessage(MessageFromMon *mes, int dataLength) {
  103. int status = 0;
  104. string header(mes->header, 4);
  105. string data(mes->data, dataLength);
  106. if (header.find(HEADER_MTS_COM_DMB) != std::string::npos) // Message pour la gestion du port de communication
  107. {
  108. if (data.find(OPEN_COM_DMB) != std::string::npos) sendAnswer(HEADER_STM_ACK, "");
  109. else if (data.find(CLOSE_COM_DMB) != std::string::npos) sendAnswer(HEADER_STM_ACK, "");
  110. } else if (header.find(HEADER_MTS_CAMERA) != std::string::npos) // Message pour la camera
  111. {
  112. if (data.find(CAM_OPEN) != std::string::npos) {
  113. sendAnswer(HEADER_STM_ACK, "");
  114. sendImage = true;
  115. } else if (data.find(CAM_CLOSE) != std::string::npos) {
  116. sendImage = false;
  117. } else if (data.find(CAM_COMPUTE_POSITION) != std::string::npos) {
  118. sendPos = true;
  119. } else if (data.find(CAM_STOP_COMPUTE_POSITION) != std::string::npos) {
  120. sendPos = false;
  121. } else {
  122. }
  123. } else if (header.find(HEADER_MTS_DMB_ORDER) != std::string::npos) // Message de com pour le robot
  124. {
  125. if (data.find(DMB_START_WITHOUT_WD) != std::string::npos) {
  126. sendAnswer(HEADER_STM_ACK, "");
  127. } else if (data.find(DMB_START_WITH_WD) != std::string::npos) {
  128. sendAnswer(HEADER_STM_ACK, "");
  129. } else if (data.find(DMB_GET_VBAT) != std::string::npos) {
  130. sendAnswer(HEADER_STM_BAT, to_string(DMB_BAT_HIGH));
  131. } else if (data.find(DMB_MOVE) != std::string::npos) {
  132. } else if (data.find(DMB_TURN) != std::string::npos) {
  133. } else {
  134. }
  135. } else if (header.find(HEADER_MTS_STOP) != std::string::npos) // Message d'arret
  136. {
  137. sendAnswer(HEADER_STM_ACK, "");
  138. } else {
  139. sendAnswer(HEADER_STM_NO_ACK, "");
  140. }
  141. return status;
  142. }
  143. int main(int argc, char** argv) {
  144. namedWindow("Sortie Camera");
  145. // Ouverture de la com robot
  146. /*if (open_communication_robot("/dev/ttyUSB0") != 0) {
  147. cerr << "Unable to open /dev/ttyUSB0: abort\n";
  148. return -1;
  149. }
  150. cout << "/dev/ttyUSB0 opened\n";
  151. */
  152. // Ouverture de la camera
  153. if (open_camera(0) == -1) {
  154. cerr << "Unable to open camera: abort\n";
  155. return -1;
  156. }
  157. cout << "Camera opened\n";
  158. // Ouverture du serveur
  159. socketID = openServer(5544);
  160. cout << "Server opened on port 5544\n";
  161. threadTimer = new std::thread(ThreadTimer);
  162. for (;;) {
  163. cout << "Waiting for client to connect ...\n";
  164. acceptClient();
  165. disconnected = false;
  166. dataReady = false;
  167. cout << "Client connected\n";
  168. threadServer = new std::thread(ThreadServer);
  169. while (disconnected == false) {
  170. // Recuperation de l'image
  171. get_image(0, &monImage, "");
  172. if (dataReady == true) // des données ont été recu par le serveur
  173. {
  174. message = (MessageFromMon*) malloc(sizeof (MessageFromMon));
  175. memcpy((void*) message, (const void*) data, sizeof (MessageFromMon));
  176. dataReady = false;
  177. //if (message->header[4] == ':') message->header[4];
  178. printReceivedMessage(message);
  179. decodeMessage(message, receivedLength - 4);
  180. free(message);
  181. }
  182. if (sysTick) {
  183. sysTick = false;
  184. if (sendImage) {
  185. compress_image(&monImage, &imageCompressed);
  186. sendAnswer(HEADER_STM_IMAGE, reinterpret_cast<char*> (imageCompressed.data()));
  187. }
  188. if (sendPos) {
  189. //sendAnswer(HEADER_STM_POS,)
  190. }
  191. }
  192. }
  193. }
  194. threadTimer->join();
  195. threadServer->join();
  196. // test de la camera
  197. if (open_camera(0) != -1) {
  198. for (;;) {
  199. get_image(0, &monImage, "");
  200. if (monImage.empty()) printf("image vide");
  201. else {
  202. imshow("Sortie Camera", monImage);
  203. waitKey(10);
  204. }
  205. }
  206. } else {
  207. printf("Echec ouverture de camera");
  208. return -1;
  209. }
  210. return 0;
  211. }