root / trunk / code / projects / colonet / server / colonet_wireless.cpp @ 623
History | View | Annotate | Download (10 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 | 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 | 117 | emarinel | for (int i = 0; i < PACKET_DATA_LEN; i++) { |
134 | 566 | emarinel | //printf("data %i: %u\n", i, args[i]);
|
135 | 117 | emarinel | pkt.data[i] = args[i]; |
136 | } |
||
137 | |||
138 | 114 | emarinel | if (dest == GLOBAL_DEST) {
|
139 | 518 | emarinel | //printf("sending to global dest\n");
|
140 | 620 | gtress | int ret = wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
141 | sizeof(ColonetRobotServerPacket), 0); |
||
142 | if (ret != 0) { |
||
143 | fprintf(stderr, "%s: wl_send_global_packet failed.\n", __FUNCTION__);
|
||
144 | 408 | emarinel | return -1; |
145 | } |
||
146 | 11 | emarinel | } else {
|
147 | 518 | emarinel | //("sending to specific robot: %d.\n", dest);
|
148 | 620 | gtress | int ret = wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
149 | sizeof(ColonetRobotServerPacket), dest,
|
||
150 | COLONET_RESPONSE_PACKET_FRAME_ID); |
||
151 | if (ret != 0) { |
||
152 | fprintf(stderr, "%s: wl_send_robot_to_robot_global_packet failed.\n", __FUNCTION__);
|
||
153 | 418 | emarinel | return -1; |
154 | } |
||
155 | 11 | emarinel | } |
156 | 409 | emarinel | |
157 | return 0; |
||
158 | 114 | emarinel | } |
159 | 11 | emarinel | |
160 | 618 | jknichel | /**
|
161 | * @brief Used to get the number of robots in the token ring
|
||
162 | *
|
||
163 | * @return The result of wl_token_get_num_robots()
|
||
164 | */
|
||
165 | 297 | emarinel | int colonet_get_num_robots(void) { |
166 | return wl_token_get_num_robots();
|
||
167 | } |
||
168 | |||
169 | 618 | jknichel | /**
|
170 | * @brief Gets a list of the xbee ids of the ones in the token ring
|
||
171 | *
|
||
172 | * @param numrobots A pointer to the variable where you want the number of robots to be stored
|
||
173 | *
|
||
174 | * @return A dynamically allocated piece of memory that contains the list of xbee ids.
|
||
175 | * The function calling this function must free this memory when done with it.
|
||
176 | */
|
||
177 | 298 | emarinel | int* colonet_get_xbee_ids(int* numrobots) { |
178 | 297 | emarinel | int num_robots = wl_token_get_num_robots();
|
179 | int* ids = (int*)malloc(num_robots * sizeof(int)); |
||
180 | |||
181 | wl_token_iterator_begin(); |
||
182 | |||
183 | 418 | emarinel | int i = 0; |
184 | 297 | emarinel | while (wl_token_iterator_has_next()) {
|
185 | 418 | emarinel | ids[i] = wl_token_iterator_next(); |
186 | i++; |
||
187 | 297 | emarinel | } |
188 | |||
189 | 298 | emarinel | *numrobots = num_robots; |
190 | 297 | emarinel | return ids;
|
191 | } |
||
192 | |||
193 | 618 | jknichel | /**
|
194 | * @brief Gets the sensor matrix from the wireless library
|
||
195 | *
|
||
196 | * @param numrobots A pointer to the variable where you want the number of robots to be stored
|
||
197 | 620 | gtress | * @param ids_ A pointer to a pointer that you want to point to the list of xbee ids. This memory is dynamically
|
198 | * allocated and should be freed by the calling process when it is done with it
|
||
199 | 618 | jknichel | *
|
200 | 620 | gtress | * @return The 2d matrix reprsenting the sensor matrix. This memory is dynamically allocated and should be freed by
|
201 | * the calling process when it is done with it
|
||
202 | 618 | jknichel | */
|
203 | 298 | emarinel | int** colonet_get_sensor_matrix(int* numrobots, int** ids_) { |
204 | 297 | emarinel | int num_robots;
|
205 | int* ids = colonet_get_xbee_ids(&num_robots);
|
||
206 | |||
207 | int** m = (int**)malloc(num_robots * sizeof(int*)); |
||
208 | for (int i = 0; i < num_robots; i++) { |
||
209 | m[i] = (int*)malloc(num_robots * sizeof(int*)); |
||
210 | } |
||
211 | |||
212 | for (int i = 0; i < num_robots; i++) { |
||
213 | for (int j = 0; j < num_robots; j++) { |
||
214 | m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
||
215 | } |
||
216 | } |
||
217 | |||
218 | 298 | emarinel | *numrobots = num_robots; |
219 | *ids_ = ids; |
||
220 | 297 | emarinel | return m;
|
221 | } |
||
222 | |||
223 | 114 | emarinel | /**************************** Private functions ******************************/
|
224 | |||
225 | static void timeout_handler() { |
||
226 | 418 | emarinel | // printf("colonet wireless - timeout!\n");
|
227 | 11 | emarinel | } |
228 | |||
229 | 114 | emarinel | static void handle_response(int frame, int received) { |
230 | 457 | jknichel | //TODO: These are just here to get rid of compiler warnings
|
231 | frame = frame; |
||
232 | received = received; |
||
233 | 623 | emarinel | printf("handle_response\n");
|
234 | 114 | emarinel | } |
235 | |||
236 | 424 | emarinel | static void handle_receive(char type, int source, unsigned char* data, int len) { |
237 | 620 | gtress | type = type; //TODO: This is just here to get rid of compiler warnings.
|
238 | 457 | jknichel | |
239 | 623 | emarinel | printf("***Got packet from robot***\n");
|
240 | 441 | jknichel | |
241 | 424 | emarinel | ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data; |
242 | 117 | emarinel | |
243 | 114 | emarinel | if (logging_enabled) {
|
244 | 424 | emarinel | log_packet(data, len); |
245 | 11 | emarinel | } |
246 | 114 | emarinel | |
247 | 618 | jknichel | //see if a robot wants the server to tell it where it is
|
248 | 444 | emarinel | if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {
|
249 | /* Robot has requested its position. */
|
||
250 | int robot_x, robot_y;
|
||
251 | 518 | emarinel | PositionMonitor* pm = colonet_server.getPositionMonitor(); |
252 | int num_robots = pm->getNumVisibleRobots();
|
||
253 | 618 | jknichel | //ask the position monitor for where that robot is.
|
254 | 518 | emarinel | int ret = pm->getRobotPosition(source, &robot_x, &robot_y);
|
255 | if (ret != 0 && num_robots != 1) { |
||
256 | 444 | emarinel | fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source);
|
257 | } else {
|
||
258 | 518 | emarinel | if (ret != 0 /* && num_robots == 1 */) { |
259 | VisionPosition pos = pm->getFirstPosition(); |
||
260 | robot_x = pos.x; |
||
261 | robot_y = pos.y; |
||
262 | } |
||
263 | |||
264 | 444 | emarinel | unsigned char response[80]; |
265 | 518 | emarinel | response[0] = (robot_x >> 8) & 0xFF; |
266 | response[1] = robot_x & 0xFF; |
||
267 | response[2] = (robot_y >> 8) & 0xFF; |
||
268 | response[3] = robot_y & 0xFF; |
||
269 | 117 | emarinel | |
270 | 518 | emarinel | fprintf(stderr, "Robot %d requested position. Its position is %d,%d.\n", source, robot_x, robot_y);
|
271 | |||
272 | 444 | emarinel | if (colonet_wl_send(-1, source, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) { |
273 | fprintf(stderr, "colonet_wl_send failed!\n");
|
||
274 | exit(1);
|
||
275 | } |
||
276 | } |
||
277 | } else {
|
||
278 | 574 | gtress | printf("Calling colonet_server.process_received_wireless_message\n");
|
279 | 444 | emarinel | char processed_data[80]; |
280 | sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source, pkt->data);
|
||
281 | 424 | emarinel | |
282 | 623 | emarinel | fprintf(stderr, "client_id=%d; processed_data: %s\n", pkt->client_id, processed_data);
|
283 | |||
284 | 444 | emarinel | colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data)); |
285 | } |
||
286 | 11 | emarinel | } |
287 | |||
288 | 114 | emarinel | static void unregister(void) { |
289 | printf("unregister\n");
|
||
290 | } |
||
291 | 11 | emarinel | |
292 | /** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
|
||
293 | * Listens for bytes on the wireless port and constructs packets based on them.
|
||
294 | * Not part of the ColonetWireless class since a function pointer must be
|
||
295 | 114 | emarinel | * passed in starting the thread that runs it (tricky or impossible if it's
|
296 | 11 | emarinel | * a class function).
|
297 | *
|
||
298 | * @param args Pointer to arguments. Can be safely casted to ListenerArgs type
|
||
299 | * @return NULL
|
||
300 | */
|
||
301 | 114 | emarinel | static void* listen(void* args) { |
302 | 620 | gtress | args = args; //TODO: These are just here to get rid of compiler warnings.
|
303 | 457 | jknichel | |
304 | 620 | gtress | fprintf(stderr, "Called listen.\n");
|
305 | 303 | emarinel | |
306 | 620 | gtress | PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID, timeout_handler, handle_response, handle_receive, unregister}; |
307 | 114 | emarinel | wl_register_packet_group(&pgh); |
308 | 11 | emarinel | |
309 | 114 | emarinel | while (1) { |
310 | wl_do(); |
||
311 | 409 | emarinel | usleep(1000);
|
312 | 11 | emarinel | } |
313 | |||
314 | 623 | emarinel | fprintf(stderr, "Listen is returning.\n");
|
315 | |||
316 | 114 | emarinel | wl_terminate(); |
317 | 11 | emarinel | return NULL; |
318 | } |
||
319 | |||
320 | 114 | emarinel | /** @brief
|
321 | 11 | emarinel | *
|
322 | * @param pkt Packet to be logged
|
||
323 | *
|
||
324 | * @return 0 on success, -1 on failure
|
||
325 | */
|
||
326 | 297 | emarinel | int log_packet(unsigned char* packet, int len) { |
327 | 11 | emarinel | FILE* logfile = fopen(log_filename, "a");
|
328 | |||
329 | if (logfile == NULL) { |
||
330 | dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
|
||
331 | return -1; |
||
332 | } |
||
333 | |||
334 | 114 | emarinel | for (int i = 0; i < len; i++) { |
335 | fprintf(logfile, "%d ", *((unsigned char*)packet + i)); |
||
336 | 11 | emarinel | } |
337 | fprintf(logfile, "\n");
|
||
338 | |||
339 | fclose(logfile); |
||
340 | return 0; |
||
341 | } |