root / trunk / code / projects / colonet / server / colonet_wireless.cpp @ 764
History | View | Annotate | Download (10.8 KB)
1 | 114 | emarinel | /** @file colonet_wireless.c
|
---|---|---|---|
2 | 11 | emarinel | *
|
3 | * @brief Implementation of server-side colonet wireless library
|
||
4 | *
|
||
5 | * @author Eugene Marinelli
|
||
6 | */
|
||
7 | |||
8 | /********************************* Includes **********************************/
|
||
9 | #include <string> |
||
10 | #include <pthread.h> |
||
11 | #include <iostream> |
||
12 | #include <fstream> |
||
13 | #include <unistd.h> |
||
14 | #include <fcntl.h> |
||
15 | 298 | emarinel | #include <signal.h> |
16 | 114 | emarinel | |
17 | 424 | emarinel | #include <ColonetServer.h> |
18 | 114 | emarinel | #include <wireless.h> // Colonet wireless library. |
19 | 297 | emarinel | #include <wl_token_ring.h> |
20 | 114 | emarinel | |
21 | 397 | emarinel | #include <colonet_defs.h> |
22 | #include <colonet_wireless.h> |
||
23 | 11 | emarinel | |
24 | /******************************* Definitions *********************************/
|
||
25 | |||
26 | 114 | emarinel | //Enable debug printouts
|
27 | 11 | emarinel | #define DEBUG 1 |
28 | |||
29 | #ifdef DEBUG
|
||
30 | #define dbg_printf(...) printf(__VA_ARGS__)
|
||
31 | #define dbg_fprintf(...) fprintf(__VA_ARGS__)
|
||
32 | #else
|
||
33 | #define dbg_printf(...)
|
||
34 | #define dbg_fprintf(...)
|
||
35 | #endif
|
||
36 | |||
37 | 424 | emarinel | extern ColonetServer colonet_server;
|
38 | |||
39 | 114 | emarinel | static bool logging_enabled; |
40 | static char log_filename[80]; |
||
41 | static pthread_t listener_thread;
|
||
42 | 117 | emarinel | static char wl_port[40]; |
43 | 114 | emarinel | |
44 | 11 | emarinel | /************************* Internal prototypes *******************************/
|
45 | static void* listen(void* args); |
||
46 | 114 | emarinel | static void timeout_handler(void); |
47 | static void handle_response(int frame, int received); |
||
48 | 348 | emarinel | static void handle_receive(char type, int source, unsigned char* packet, int len); |
49 | 114 | emarinel | static void unregister(void); |
50 | 115 | emarinel | static int log_packet(unsigned char* packet, int len); |
51 | 11 | emarinel | |
52 | /**************************** Public functions *******************************/
|
||
53 | 618 | jknichel | /**
|
54 | * @brief Initializes the wireless library
|
||
55 | *
|
||
56 | * @param wl_port_ The port to listen for wireless messages on
|
||
57 | * @param log_filename_ The name of the log file
|
||
58 | *
|
||
59 | * @return 0 on success, negative error code on failure
|
||
60 | */
|
||
61 | 424 | emarinel | int colonet_wl_init(char* wl_port_, char* log_filename_) { |
62 | 347 | emarinel | if (log_filename_ != NULL) { |
63 | 11 | emarinel | logging_enabled = true;
|
64 | 347 | emarinel | strcpy(log_filename, log_filename_); |
65 | 11 | emarinel | } |
66 | |||
67 | 117 | emarinel | strncpy(wl_port, wl_port_, 40);
|
68 | |||
69 | 399 | emarinel | wl_set_com_port(wl_port); |
70 | 303 | emarinel | |
71 | 623 | emarinel | fprintf(stderr, "Calling wl_init(%s)...\n", wl_port);
|
72 | 399 | emarinel | if (wl_init() != 0) { |
73 | fprintf(stderr, "wl_init failed.\n");
|
||
74 | return -1; |
||
75 | } |
||
76 | |||
77 | 297 | emarinel | wl_token_ring_register(); |
78 | 303 | emarinel | |
79 | 623 | emarinel | fprintf(stderr, "Joining token ring...\n");
|
80 | 397 | emarinel | if (wl_token_ring_join() != 0) { |
81 | fprintf(stderr, "Failed to join token ring.\n");
|
||
82 | return -1; |
||
83 | } |
||
84 | 623 | emarinel | fprintf(stderr, "Joined token ring.\n");
|
85 | 397 | emarinel | |
86 | return 0; |
||
87 | 11 | emarinel | } |
88 | |||
89 | 618 | jknichel | /**
|
90 | * @brief This function will kill the thread that is listening for wireless messages
|
||
91 | */
|
||
92 | 114 | emarinel | void colonet_wl_kill_listener_thread() {
|
93 | pthread_kill(listener_thread, 9);
|
||
94 | 11 | emarinel | } |
95 | |||
96 | 618 | jknichel | /**
|
97 | * @brief This function spawns a thread to listen for wireless messages
|
||
98 | *
|
||
99 | * @return 0 on success, negative error code on failure
|
||
100 | */
|
||
101 | 117 | emarinel | int colonet_wl_run_listener_thread() {
|
102 | 11 | emarinel | dbg_printf("Spawning listener thread...\n");
|
103 | |||
104 | 115 | emarinel | if (pthread_create(&listener_thread, NULL, listen, NULL)) { |
105 | 11 | emarinel | perror("pthread_create");
|
106 | 117 | emarinel | return -1; |
107 | 11 | emarinel | } |
108 | 117 | emarinel | |
109 | return 0; |
||
110 | 11 | emarinel | } |
111 | |||
112 | 618 | jknichel | /**
|
113 | * @brief This function sends a message out on the wireless
|
||
114 | *
|
||
115 | * @param client_source The index in the connection pool of the client that is sending the message
|
||
116 | * @param dest The robot that is meant to receive this message
|
||
117 | * @param msg_type The type of the message
|
||
118 | * @param msg_code The message code
|
||
119 | * @param args The arguments to send with the message
|
||
120 | *
|
||
121 | * @return 0 on success, a negative error code on failure
|
||
122 | */
|
||
123 | 424 | emarinel | int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code, |
124 | 620 | gtress | unsigned char* args) { |
125 | 648 | emarinel | printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
|
126 | 175 | emarinel | |
127 | 620 | gtress | //create and set up new packet to be sent.
|
128 | 117 | emarinel | ColonetRobotServerPacket pkt; |
129 | pkt.client_id = client_source; |
||
130 | pkt.msg_code = msg_code; |
||
131 | 167 | emarinel | |
132 | 618 | jknichel | //copy over the args into the data portion of the packet
|
133 | 648 | emarinel | if (args != NULL) { |
134 | for (int i = 0; i < PACKET_DATA_LEN; i++) { |
||
135 | printf("data %i: %u\n", i, args[i]);
|
||
136 | pkt.data[i] = args[i]; |
||
137 | } |
||
138 | 117 | emarinel | } |
139 | |||
140 | 114 | emarinel | if (dest == GLOBAL_DEST) {
|
141 | 518 | emarinel | //printf("sending to global dest\n");
|
142 | 620 | gtress | int ret = wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
143 | sizeof(ColonetRobotServerPacket), 0); |
||
144 | if (ret != 0) { |
||
145 | fprintf(stderr, "%s: wl_send_global_packet failed.\n", __FUNCTION__);
|
||
146 | 408 | emarinel | return -1; |
147 | } |
||
148 | 11 | emarinel | } else {
|
149 | 518 | emarinel | //("sending to specific robot: %d.\n", dest);
|
150 | 620 | gtress | int ret = wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
151 | sizeof(ColonetRobotServerPacket), dest,
|
||
152 | COLONET_RESPONSE_PACKET_FRAME_ID); |
||
153 | if (ret != 0) { |
||
154 | fprintf(stderr, "%s: wl_send_robot_to_robot_global_packet failed.\n", __FUNCTION__);
|
||
155 | 418 | emarinel | return -1; |
156 | } |
||
157 | 11 | emarinel | } |
158 | 409 | emarinel | |
159 | return 0; |
||
160 | 114 | emarinel | } |
161 | 11 | emarinel | |
162 | 618 | jknichel | /**
|
163 | * @brief Used to get the number of robots in the token ring
|
||
164 | *
|
||
165 | * @return The result of wl_token_get_num_robots()
|
||
166 | */
|
||
167 | 297 | emarinel | int colonet_get_num_robots(void) { |
168 | return wl_token_get_num_robots();
|
||
169 | } |
||
170 | |||
171 | 618 | jknichel | /**
|
172 | * @brief Gets a list of the xbee ids of the ones in the token ring
|
||
173 | *
|
||
174 | * @param numrobots A pointer to the variable where you want the number of robots to be stored
|
||
175 | *
|
||
176 | * @return A dynamically allocated piece of memory that contains the list of xbee ids.
|
||
177 | * The function calling this function must free this memory when done with it.
|
||
178 | */
|
||
179 | 298 | emarinel | int* colonet_get_xbee_ids(int* numrobots) { |
180 | 297 | emarinel | int num_robots = wl_token_get_num_robots();
|
181 | int* ids = (int*)malloc(num_robots * sizeof(int)); |
||
182 | |||
183 | wl_token_iterator_begin(); |
||
184 | |||
185 | 418 | emarinel | int i = 0; |
186 | 297 | emarinel | while (wl_token_iterator_has_next()) {
|
187 | 418 | emarinel | ids[i] = wl_token_iterator_next(); |
188 | i++; |
||
189 | 297 | emarinel | } |
190 | |||
191 | 298 | emarinel | *numrobots = num_robots; |
192 | 297 | emarinel | return ids;
|
193 | } |
||
194 | |||
195 | 618 | jknichel | /**
|
196 | * @brief Gets the sensor matrix from the wireless library
|
||
197 | *
|
||
198 | * @param numrobots A pointer to the variable where you want the number of robots to be stored
|
||
199 | 620 | gtress | * @param ids_ A pointer to a pointer that you want to point to the list of xbee ids. This memory is dynamically
|
200 | * allocated and should be freed by the calling process when it is done with it
|
||
201 | 618 | jknichel | *
|
202 | 620 | gtress | * @return The 2d matrix reprsenting the sensor matrix. This memory is dynamically allocated and should be freed by
|
203 | * the calling process when it is done with it
|
||
204 | 618 | jknichel | */
|
205 | 298 | emarinel | int** colonet_get_sensor_matrix(int* numrobots, int** ids_) { |
206 | 297 | emarinel | int num_robots;
|
207 | int* ids = colonet_get_xbee_ids(&num_robots);
|
||
208 | |||
209 | int** m = (int**)malloc(num_robots * sizeof(int*)); |
||
210 | for (int i = 0; i < num_robots; i++) { |
||
211 | m[i] = (int*)malloc(num_robots * sizeof(int*)); |
||
212 | } |
||
213 | |||
214 | for (int i = 0; i < num_robots; i++) { |
||
215 | for (int j = 0; j < num_robots; j++) { |
||
216 | m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
||
217 | } |
||
218 | } |
||
219 | |||
220 | 298 | emarinel | *numrobots = num_robots; |
221 | *ids_ = ids; |
||
222 | 297 | emarinel | return m;
|
223 | } |
||
224 | |||
225 | 114 | emarinel | /**************************** Private functions ******************************/
|
226 | |||
227 | 629 | jknichel | //TODO: fill in the doxygen comments
|
228 | /**
|
||
229 | 710 | gtress | * @brief
|
230 | 629 | jknichel | *
|
231 | * @return Void
|
||
232 | */
|
||
233 | 114 | emarinel | static void timeout_handler() { |
234 | 418 | emarinel | // printf("colonet wireless - timeout!\n");
|
235 | 11 | emarinel | } |
236 | |||
237 | 629 | jknichel | //TODO: fill in the doxygen comments
|
238 | /**
|
||
239 | 710 | gtress | * @brief
|
240 | 629 | jknichel | *
|
241 | * @param frame
|
||
242 | * @param received
|
||
243 | *
|
||
244 | * @return Void
|
||
245 | */
|
||
246 | 114 | emarinel | static void handle_response(int frame, int received) { |
247 | 457 | jknichel | //TODO: These are just here to get rid of compiler warnings
|
248 | frame = frame; |
||
249 | received = received; |
||
250 | 114 | emarinel | } |
251 | |||
252 | 629 | jknichel | /**
|
253 | * @brief When a packet is received via wireless, the data is passed to this function
|
||
254 | *
|
||
255 | 710 | gtress | * @param type
|
256 | 629 | jknichel | * @param source The robot the packet came from
|
257 | * @param data The data the robot sent
|
||
258 | * @param len The length of the data
|
||
259 | *
|
||
260 | * @return Void
|
||
261 | */
|
||
262 | 648 | emarinel | static void handle_receive(char type, int source_robot, unsigned char* data, int len) { |
263 | 620 | gtress | type = type; //TODO: This is just here to get rid of compiler warnings.
|
264 | 457 | jknichel | |
265 | 681 | emarinel | //printf("***Got packet from robot***\n");
|
266 | 441 | jknichel | |
267 | 424 | emarinel | ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data; |
268 | 117 | emarinel | |
269 | 114 | emarinel | if (logging_enabled) {
|
270 | 424 | emarinel | log_packet(data, len); |
271 | 11 | emarinel | } |
272 | 114 | emarinel | |
273 | 618 | jknichel | //see if a robot wants the server to tell it where it is
|
274 | 444 | emarinel | if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {
|
275 | /* Robot has requested its position. */
|
||
276 | int robot_x, robot_y;
|
||
277 | 518 | emarinel | PositionMonitor* pm = colonet_server.getPositionMonitor(); |
278 | int num_robots = pm->getNumVisibleRobots();
|
||
279 | 629 | jknichel | |
280 | 618 | jknichel | //ask the position monitor for where that robot is.
|
281 | 648 | emarinel | int ret = pm->getRobotPosition(source_robot, &robot_x, &robot_y);
|
282 | 518 | emarinel | if (ret != 0 && num_robots != 1) { |
283 | 710 | gtress | //DEBUG
|
284 | //fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source_robot);
|
||
285 | 648 | emarinel | |
286 | if (colonet_wl_send(-1, source_robot, COLONET_COMMAND, SERVER_REPORT_ROBOT_LOST, NULL) != 0) { |
||
287 | fprintf(stderr, "colonet_wl_send failed!\n");
|
||
288 | exit(1);
|
||
289 | } |
||
290 | 444 | emarinel | } else {
|
291 | 648 | emarinel | if (ret != 0 && num_robots == 1) { |
292 | 518 | emarinel | VisionPosition pos = pm->getFirstPosition(); |
293 | robot_x = pos.x; |
||
294 | robot_y = pos.y; |
||
295 | } |
||
296 | |||
297 | 444 | emarinel | unsigned char response[80]; |
298 | 518 | emarinel | response[0] = (robot_x >> 8) & 0xFF; |
299 | response[1] = robot_x & 0xFF; |
||
300 | response[2] = (robot_y >> 8) & 0xFF; |
||
301 | response[3] = robot_y & 0xFF; |
||
302 | 117 | emarinel | |
303 | 648 | emarinel | fprintf(stderr, "Robot %d requested position. Its position is %d,%d.\n", source_robot, robot_x, robot_y);
|
304 | 518 | emarinel | |
305 | 648 | emarinel | if (colonet_wl_send(-1, source_robot, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) { |
306 | 444 | emarinel | fprintf(stderr, "colonet_wl_send failed!\n");
|
307 | exit(1);
|
||
308 | } |
||
309 | } |
||
310 | } else {
|
||
311 | 631 | emarinel | //printf("Calling colonet_server.process_received_wireless_message\n");
|
312 | 444 | emarinel | char processed_data[80]; |
313 | 648 | emarinel | sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source_robot, pkt->data);
|
314 | 424 | emarinel | |
315 | 631 | emarinel | //fprintf(stderr, "client_id=%d; processed_data: %s\n", pkt->client_id, processed_data);
|
316 | 623 | emarinel | |
317 | 444 | emarinel | colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data)); |
318 | } |
||
319 | 11 | emarinel | } |
320 | |||
321 | 629 | jknichel | //TODO: fill in the doxygen comments
|
322 | /**
|
||
323 | 710 | gtress | * @brief
|
324 | 629 | jknichel | *
|
325 | * @return Void
|
||
326 | */
|
||
327 | 114 | emarinel | static void unregister(void) { |
328 | printf("unregister\n");
|
||
329 | } |
||
330 | 11 | emarinel | |
331 | /** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
|
||
332 | * Listens for bytes on the wireless port and constructs packets based on them.
|
||
333 | * Not part of the ColonetWireless class since a function pointer must be
|
||
334 | 114 | emarinel | * passed in starting the thread that runs it (tricky or impossible if it's
|
335 | 11 | emarinel | * a class function).
|
336 | *
|
||
337 | * @param args Pointer to arguments. Can be safely casted to ListenerArgs type
|
||
338 | * @return NULL
|
||
339 | */
|
||
340 | 114 | emarinel | static void* listen(void* args) { |
341 | 620 | gtress | args = args; //TODO: These are just here to get rid of compiler warnings.
|
342 | 457 | jknichel | |
343 | 620 | gtress | fprintf(stderr, "Called listen.\n");
|
344 | 303 | emarinel | |
345 | 620 | gtress | PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, timeout_handler, handle_response, handle_receive, unregister}; |
346 | 114 | emarinel | wl_register_packet_group(&pgh); |
347 | 11 | emarinel | |
348 | 114 | emarinel | while (1) { |
349 | wl_do(); |
||
350 | 629 | jknichel | |
351 | //this sleep is here so the thread this runs in doesn't hog the cpu
|
||
352 | 409 | emarinel | usleep(1000);
|
353 | 11 | emarinel | } |
354 | |||
355 | 623 | emarinel | fprintf(stderr, "Listen is returning.\n");
|
356 | |||
357 | 114 | emarinel | wl_terminate(); |
358 | 11 | emarinel | return NULL; |
359 | } |
||
360 | |||
361 | 710 | gtress | /**
|
362 | 629 | jknichel | * @brief
|
363 | 11 | emarinel | *
|
364 | * @param pkt Packet to be logged
|
||
365 | *
|
||
366 | * @return 0 on success, -1 on failure
|
||
367 | */
|
||
368 | 297 | emarinel | int log_packet(unsigned char* packet, int len) { |
369 | 11 | emarinel | FILE* logfile = fopen(log_filename, "a");
|
370 | |||
371 | if (logfile == NULL) { |
||
372 | dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
|
||
373 | return -1; |
||
374 | } |
||
375 | |||
376 | 114 | emarinel | for (int i = 0; i < len; i++) { |
377 | fprintf(logfile, "%d ", *((unsigned char*)packet + i)); |
||
378 | 11 | emarinel | } |
379 | fprintf(logfile, "\n");
|
||
380 | |||
381 | fclose(logfile); |
||
382 | return 0; |
||
383 | } |