Browse Source

simulateur: ajout temps de réponse et erreur, comrobot : ajout msgAnswer adequat a la reponse

pehladik 4 years ago
parent
commit
7f0a9955db
2 changed files with 139 additions and 77 deletions
  1. 15
    8
      software/raspberry/superviseur-robot/lib/comrobot.cpp
  2. 124
    69
      software/simulateur/main.cpp

+ 15
- 8
software/raspberry/superviseur-robot/lib/comrobot.cpp View File

@@ -80,12 +80,16 @@ int ComRobot::Open(string usart) {
80 80
     struct termios options;
81 81
 
82 82
 #ifdef __SIMULATION__
83
-    
83
+
84 84
     struct sockaddr_in serv_addr;
85 85
     if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
86 86
         printf("\n Socket creation error \n");
87 87
         return -1;
88 88
     }
89
+    struct timeval tv;
90
+    tv.tv_sec = 0;
91
+    tv.tv_usec = 80000;
92
+    setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv);
89 93
 
90 94
     serv_addr.sin_family = AF_INET;
91 95
     serv_addr.sin_port = htons(PORT);
@@ -99,10 +103,6 @@ int ComRobot::Open(string usart) {
99 103
     if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) {
100 104
         return -1;
101 105
     }
102
-    /*send(sock , hello , strlen(hello) , 0 ); 
103
-    printf("Hello message sent\n"); 
104
-    valread = read( sock , buffer, 1024); 
105
-    printf("%s\n",buffer ); */
106 106
     return 1;
107 107
 #else
108 108
 
@@ -154,11 +154,18 @@ Message *ComRobot::Write(Message* msg) {
154 154
 #ifdef __SIMULATION__
155 155
 
156 156
         char buffer[1024] = {0};
157
-        cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<<s<<endl<<flush;
157
+        cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush;
158 158
         send(sock, s.c_str(), s.length(), 0);
159
+
159 160
         int valread = read(sock, buffer, 1024);
160
-        msgAnswer = new Message(MESSAGE_ANSWER_ACK);
161
-        printf("%s\n", buffer);
161
+        if (valread < 0) {
162
+            msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_TIMEOUT);
163
+        } else {
164
+            string s(&buffer[0], valread);
165
+            msgAnswer = StringToMessage(s);
166
+            //msgAnswer = new Message(MESSAGE_ANSWER_ACK);
167
+        }
168
+        cout << "response: " <<  buffer << ", id: " << msgAnswer->GetID() << endl;
162 169
 #else       
163 170
         AddChecksum(s);
164 171
 

+ 124
- 69
software/simulateur/main.cpp View File

@@ -5,6 +5,7 @@
5 5
 #include <netinet/in.h> 
6 6
 #include <string.h> 
7 7
 #include <iostream>
8
+#include <time.h>
8 9
 using namespace std;
9 10
 
10 11
 const char LABEL_ROBOT_PING = 'p';
@@ -25,14 +26,47 @@ const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C';
25 26
 const char LABEL_ROBOT_SEPARATOR_CHAR = '=';
26 27
 const char LABEL_ROBOT_ENDING_CHAR = 0x0D;
27 28
 
29
+int server_fd, new_socket, valread;
28 30
 #define PORT 6699
29 31
 
30
-int main(int argc, char const *argv[]) {
31
-    int server_fd, new_socket, valread;
32
+long int ellapse(struct timespec ref, struct timespec cur) {
33
+    long int e;
34
+    e = cur.tv_sec - ref.tv_sec;
35
+    e *= 1000000000;
36
+    e += cur.tv_nsec - ref.tv_nsec;
37
+    return e;
38
+}
39
+
40
+void print_time(struct timespec start_time) {
41
+    struct timespec t;
42
+    clock_gettime(CLOCK_REALTIME, &t);
43
+    long int e = ellapse(start_time, t);
44
+    fprintf(stdout, "%9ld", e / 1000000);
45
+}
46
+
47
+int simulate_error() {
48
+    int r = rand() % 1000;
49
+    if (r > 950) {
50
+        printf(">>> I don't understand what you said (-1)\n");
51
+        return -1;
52
+    } else if (r > 900) {
53
+        printf(">>> I don't want to respond (-2)\n");
54
+        return -2;
55
+    }
56
+    printf(">>> WILCO (0)\n");
57
+    return 0;
58
+}
59
+
60
+void simulate_transmission_time() {
61
+    usleep((rand() % 30) * 1000);
62
+}
63
+
64
+void open_server() {
32 65
     struct sockaddr_in address;
33 66
     int opt = 1;
34 67
     int addrlen = sizeof (address);
35
-    char buffer[1024] = {0};
68
+
69
+    srand(time(NULL));
36 70
 
37 71
     // Creating socket file descriptor 
38 72
     if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) {
@@ -58,90 +92,111 @@ int main(int argc, char const *argv[]) {
58 92
         perror("bind failed");
59 93
         exit(EXIT_FAILURE);
60 94
     }
61
-    cout << ">>> waiting the opening" << endl;
95
+    cout << ">>> I create a server" << endl;
96
+    cout << ">>> ..." << endl;
97
+    cout << ">>> I'm waiting a client" << endl;
62 98
     if (listen(server_fd, 3) < 0) {
63 99
         perror("listen");
64 100
         exit(EXIT_FAILURE);
65 101
     }
102
+
103
+    cout << ">>> Hello, I'm Robot" << endl;
66 104
     if ((new_socket = accept(server_fd, (struct sockaddr *) &address,
67 105
             (socklen_t*) & addrlen)) < 0) {
68 106
         perror("accept");
69 107
         exit(EXIT_FAILURE);
70 108
     }
71
-    cout << ">>> The robot is ready to receive something" << endl;
72
-    unsigned long starttime = (unsigned long) time(NULL);
109
+}
110
+
111
+int main(int argc, char const *argv[]) {
112
+
113
+    char buffer[1024] = {0};
114
+    open_server();
115
+    cout << ">>> I'm ready to receive something" << endl;
116
+    struct timespec start_time;
117
+    clock_gettime(CLOCK_REALTIME, &start_time);
118
+
73 119
     while (1) {
74 120
         valread = read(new_socket, buffer, 1024);
75 121
         if (valread <= 0)
76 122
             break;
77
-        printf(">>> I received : %s\n", buffer);
78
-        cout << (unsigned long) time(NULL) << ": ";
123
+        print_time(start_time);
124
+        printf(" >>> I received a message : %s\n", buffer);
79 125
         string s = "";
80
-        s += LABEL_ROBOT_OK;
81
-        switch (buffer[0]) {
82
-            case LABEL_ROBOT_START_WITHOUT_WD:
83
-                s += LABEL_ROBOT_OK;
84
-                break;
85
-            case LABEL_ROBOT_START_WITH_WD:
86
-                s += LABEL_ROBOT_OK;
87
-                break;
88
-            case LABEL_ROBOT_MOVE:
89
-                switch (buffer[2]) {
90
-                    case '0':
91
-                        cout << "oo I stop oo" << endl;
92
-                        break;
93
-                    case '-':
94
-                        cout << "\\/ I move backward \\/" << endl;
95
-                        break;
96
-                    default:
97
-                        cout << "/\\ I move forward /\\" << endl;
98
-                        break;
99
-                }
100
-                s += LABEL_ROBOT_OK;
101
-                break;
102
-            case LABEL_ROBOT_TURN:
103
-                switch (buffer[2]) {
104
-                    case '-':
105
-                        cout << "<< I turn to the left <<" << endl;
106
-                        break;
107
-                    default:
108
-                        cout << ">> I turn to the right >>" << endl;
109
-                        break;
110
-                }
111
-                s += LABEL_ROBOT_OK;
112
-                break;
113
-            case LABEL_ROBOT_GET_BATTERY:
114
-                if ((unsigned long) time(NULL) - starttime > 20) {
115
-                    s += '0';
116
-                } else {
117
-                    if ((unsigned long) time(NULL) - starttime > 10) {
118
-                        s += '1';
126
+        int error = simulate_error();
127
+        if (error == 0) {
128
+            struct timespec t;
129
+            long int e;
130
+            switch (buffer[0]) {
131
+                case LABEL_ROBOT_START_WITHOUT_WD:
132
+                    cout << ">>> I start without watchdog" << endl;
133
+                    s += LABEL_ROBOT_OK;
134
+                    break;
135
+                case LABEL_ROBOT_START_WITH_WD:
136
+                    s += LABEL_ROBOT_OK;
137
+                    break;
138
+                case LABEL_ROBOT_MOVE:
139
+                    switch (buffer[2]) {
140
+                        case '0':
141
+                            cout << ">>> XX I stop XX" << endl;
142
+                            break;
143
+                        case '-':
144
+                            cout << ">>> \\/ I move backward \\/" << endl;
145
+                            break;
146
+                        default:
147
+                            cout << ">>> /\\ I move forward /\\" << endl;
148
+                            break;
149
+                    }
150
+                    s += LABEL_ROBOT_OK;
151
+                    break;
152
+                case LABEL_ROBOT_TURN:
153
+                    switch (buffer[2]) {
154
+                        case '-':
155
+                            cout << ">>> << I turn to the left <<" << endl;
156
+                            break;
157
+                        default:
158
+                            cout << ">>> >> I turn to the right >>" << endl;
159
+                            break;
160
+                    }
161
+                    s += LABEL_ROBOT_OK;
162
+                    break;
163
+                case LABEL_ROBOT_GET_BATTERY:
164
+                    cout << ">>> I give you my battery level :-o" << endl;
165
+                    clock_gettime(CLOCK_REALTIME, &t);
166
+                    e = ellapse(start_time, t);
167
+                    if (e > 20000000000) {
168
+                        s += '0';
119 169
                     } else {
120
-                        s += '2';
170
+                        if (e > 10000000000) {
171
+                            s += '1';
172
+                        } else {
173
+                            s += '2';
174
+                        }
121 175
                     }
122
-                }
123
-
124
-
125
-
126
-                break;
127
-            case '0':
128
-                //msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY);
129
-                break;
130
-            case '1':
131
-                //msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW);
132
-                break;
133
-            case '2':
134
-                //msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL);
135
-                break;
136
-            default:
137
-                //msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR);
138
-                cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << buffer << ")" << endl << flush;
176
+                    break;
177
+                case LABEL_ROBOT_RELOAD_WD:
178
+                    cout << ">>> I start with watchdog" << endl;
179
+                    s += LABEL_ROBOT_OK;
180
+                    break;
181
+                case LABEL_ROBOT_POWEROFF:
182
+                    cout << ">>> Bye bye" << endl;
183
+                    s += LABEL_ROBOT_OK;
184
+                    break;
185
+                default:
186
+                    //msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR);
187
+                    cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << buffer << ")" << endl << flush;
188
+            }
189
+            simulate_transmission_time();
190
+            send(new_socket, s.c_str(), s.length(), 0);
191
+        } else if (error == -1){
192
+            s += LABEL_ROBOT_UNKNOWN_COMMAND;
193
+            simulate_transmission_time();
194
+            send(new_socket, s.c_str(), s.length(), 0);
195
+        } else if (error == -2){
196
+            /* Do nothing */
139 197
         }
140
-
141
-        send(new_socket, s.c_str(), s.length(), 0);
142
-        //printf("%s sent\n", s);
143 198
     }
144
-    cout << "The robot is dead. End of story " << endl;
199
+    cout << "The robot is dead. End of story. " << endl;
145 200
     cout << " /\\_/\\" << endl << "( o.o )" << endl << " > ^ <" << endl;
146 201
     return 0;
147 202
 } 

Loading…
Cancel
Save