root / trunk / code / projects / colonet / server / colonet_wireless.cpp @ 681
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 | * @brief
|
||
230 | *
|
||
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 | * @brief
|
||
240 | *
|
||
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 | * @param type
|
||
256 | * @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 | 648 | emarinel | fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source_robot);
|
284 | |||
285 | if (colonet_wl_send(-1, source_robot, COLONET_COMMAND, SERVER_REPORT_ROBOT_LOST, NULL) != 0) { |
||
286 | fprintf(stderr, "colonet_wl_send failed!\n");
|
||
287 | exit(1);
|
||
288 | } |
||
289 | 444 | emarinel | } else {
|
290 | 648 | emarinel | if (ret != 0 && num_robots == 1) { |
291 | 518 | emarinel | VisionPosition pos = pm->getFirstPosition(); |
292 | robot_x = pos.x; |
||
293 | robot_y = pos.y; |
||
294 | } |
||
295 | |||
296 | 444 | emarinel | unsigned char response[80]; |
297 | 518 | emarinel | response[0] = (robot_x >> 8) & 0xFF; |
298 | response[1] = robot_x & 0xFF; |
||
299 | response[2] = (robot_y >> 8) & 0xFF; |
||
300 | response[3] = robot_y & 0xFF; |
||
301 | 117 | emarinel | |
302 | 648 | emarinel | fprintf(stderr, "Robot %d requested position. Its position is %d,%d.\n", source_robot, robot_x, robot_y);
|
303 | 518 | emarinel | |
304 | 648 | emarinel | if (colonet_wl_send(-1, source_robot, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) { |
305 | 444 | emarinel | fprintf(stderr, "colonet_wl_send failed!\n");
|
306 | exit(1);
|
||
307 | } |
||
308 | } |
||
309 | } else {
|
||
310 | 631 | emarinel | //printf("Calling colonet_server.process_received_wireless_message\n");
|
311 | 444 | emarinel | char processed_data[80]; |
312 | 648 | emarinel | sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source_robot, pkt->data);
|
313 | 424 | emarinel | |
314 | 631 | emarinel | //fprintf(stderr, "client_id=%d; processed_data: %s\n", pkt->client_id, processed_data);
|
315 | 623 | emarinel | |
316 | 444 | emarinel | colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data)); |
317 | } |
||
318 | 11 | emarinel | } |
319 | |||
320 | 629 | jknichel | //TODO: fill in the doxygen comments
|
321 | /**
|
||
322 | * @brief
|
||
323 | *
|
||
324 | * @return Void
|
||
325 | */
|
||
326 | 114 | emarinel | static void unregister(void) { |
327 | printf("unregister\n");
|
||
328 | } |
||
329 | 11 | emarinel | |
330 | /** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
|
||
331 | * Listens for bytes on the wireless port and constructs packets based on them.
|
||
332 | * Not part of the ColonetWireless class since a function pointer must be
|
||
333 | 114 | emarinel | * passed in starting the thread that runs it (tricky or impossible if it's
|
334 | 11 | emarinel | * a class function).
|
335 | *
|
||
336 | * @param args Pointer to arguments. Can be safely casted to ListenerArgs type
|
||
337 | * @return NULL
|
||
338 | */
|
||
339 | 114 | emarinel | static void* listen(void* args) { |
340 | 620 | gtress | args = args; //TODO: These are just here to get rid of compiler warnings.
|
341 | 457 | jknichel | |
342 | 620 | gtress | fprintf(stderr, "Called listen.\n");
|
343 | 303 | emarinel | |
344 | 620 | gtress | PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, timeout_handler, handle_response, handle_receive, unregister}; |
345 | 114 | emarinel | wl_register_packet_group(&pgh); |
346 | 11 | emarinel | |
347 | 114 | emarinel | while (1) { |
348 | wl_do(); |
||
349 | 629 | jknichel | |
350 | //this sleep is here so the thread this runs in doesn't hog the cpu
|
||
351 | 409 | emarinel | usleep(1000);
|
352 | 11 | emarinel | } |
353 | |||
354 | 623 | emarinel | fprintf(stderr, "Listen is returning.\n");
|
355 | |||
356 | 114 | emarinel | wl_terminate(); |
357 | 11 | emarinel | return NULL; |
358 | } |
||
359 | |||
360 | 629 | jknichel | /**
|
361 | * @brief
|
||
362 | 11 | emarinel | *
|
363 | * @param pkt Packet to be logged
|
||
364 | *
|
||
365 | * @return 0 on success, -1 on failure
|
||
366 | */
|
||
367 | 297 | emarinel | int log_packet(unsigned char* packet, int len) { |
368 | 11 | emarinel | FILE* logfile = fopen(log_filename, "a");
|
369 | |||
370 | if (logfile == NULL) { |
||
371 | dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
|
||
372 | return -1; |
||
373 | } |
||
374 | |||
375 | 114 | emarinel | for (int i = 0; i < len; i++) { |
376 | fprintf(logfile, "%d ", *((unsigned char*)packet + i)); |
||
377 | 11 | emarinel | } |
378 | fprintf(logfile, "\n");
|
||
379 | |||
380 | fclose(logfile); |
||
381 | return 0; |
||
382 | } |