root / branches / simulator / projects / simulator / simulator / core / robot.c @ 1076
History | View | Annotate | Download (7.46 KB)
1 | 988 | bcoltin | /**
|
---|---|---|---|
2 | * @file robot.c
|
||
3 | * @author Colony Project
|
||
4 | * @brief Simulator Robot Management
|
||
5 | *
|
||
6 | * Contains implementations of functions managing robots
|
||
7 | * in the simulator, including process control and memory
|
||
8 | * management.
|
||
9 | **/
|
||
10 | |||
11 | #include <stdlib.h> |
||
12 | #include <stdio.h> |
||
13 | #include <string.h> |
||
14 | |||
15 | #include <unistd.h> |
||
16 | #include <signal.h> |
||
17 | #include <sys/wait.h> |
||
18 | #include <robot_shared.h> |
||
19 | #include <sys/ipc.h> |
||
20 | #include <sys/types.h> |
||
21 | #include <sys/shm.h> |
||
22 | #include <errno.h> |
||
23 | 1006 | bcoltin | #include <pthread.h> |
24 | 1039 | bneuman | #include <err.h> |
25 | 1068 | ayeager | #include <unistd.h> |
26 | 988 | bcoltin | |
27 | 1064 | bcoltin | #include "gtk_gui.h" |
28 | |||
29 | 988 | bcoltin | #include "robot.h" |
30 | 1010 | bcoltin | #include "motion.h" |
31 | 1064 | bcoltin | #include "rangefinders.h" |
32 | 1044 | bpoole | #include "world.h" |
33 | 1068 | ayeager | #include "logger.h" |
34 | #include "gtk_gui.h" |
||
35 | 988 | bcoltin | |
36 | 1068 | ayeager | |
37 | 988 | bcoltin | void sig_chld_handler(int sig); |
38 | 1010 | bcoltin | void robot_update(int i); |
39 | 988 | bcoltin | |
40 | // global variables
|
||
41 | int first_available_id = 0; |
||
42 | // an expanding array of robots
|
||
43 | Robot* robots = NULL;
|
||
44 | int robots_size = 0; |
||
45 | 1006 | bcoltin | int num_robots = 0; |
46 | 988 | bcoltin | |
47 | 1072 | bneuman | int iterator_pos = 0; |
48 | 997 | bcoltin | |
49 | 1068 | ayeager | int timeStep = 0; |
50 | |||
51 | 1006 | bcoltin | // signalled when all robots have run for a bit
|
52 | pthread_mutex_t all_finished_mutex; |
||
53 | pthread_cond_t all_finished_cond; |
||
54 | int finished = 0; |
||
55 | |||
56 | 1074 | bcoltin | int pausing = 0; |
57 | int paused = 0; |
||
58 | |||
59 | 988 | bcoltin | int robots_initialize(void) |
60 | { |
||
61 | 997 | bcoltin | int i;
|
62 | 988 | bcoltin | if (signal(SIGCHLD, sig_chld_handler) == SIG_ERR)
|
63 | return -1; |
||
64 | |||
65 | robots_size = 10;
|
||
66 | robots = (Robot*)malloc(sizeof(Robot) * robots_size);
|
||
67 | if (!robots)
|
||
68 | return -1; |
||
69 | memset(robots, 0, sizeof(Robot) * robots_size); |
||
70 | 997 | bcoltin | for (i = 0; i < robots_size; i++) |
71 | robots[i].id = -1;
|
||
72 | 1072 | bneuman | |
73 | 1006 | bcoltin | finished = 0;
|
74 | pthread_mutex_init(&all_finished_mutex, NULL);
|
||
75 | pthread_cond_init(&all_finished_cond, NULL);
|
||
76 | 988 | bcoltin | |
77 | return 0; |
||
78 | } |
||
79 | |||
80 | 1074 | bcoltin | int robots_cleanup(void) |
81 | { |
||
82 | robots_pause(); |
||
83 | //TODO: clean things up
|
||
84 | return 0; |
||
85 | } |
||
86 | |||
87 | void robots_pause(void) |
||
88 | { |
||
89 | pausing = 1;
|
||
90 | while (!paused) usleep(10000); |
||
91 | } |
||
92 | |||
93 | void robots_resume(void) |
||
94 | { |
||
95 | pausing = 0;
|
||
96 | } |
||
97 | |||
98 | 988 | bcoltin | /**
|
99 | * Creates a new robot. Returns its id on
|
||
100 | * success, or a negative integer on failure.
|
||
101 | **/
|
||
102 | int robot_create(char *execname) |
||
103 | { |
||
104 | 1068 | ayeager | int pid,i;
|
105 | 988 | bcoltin | int id = first_available_id;
|
106 | Robot* r = &robots[id]; |
||
107 | // do shared memory stuff here
|
||
108 | key_t key = IPC_PRIVATE; |
||
109 | //Memory accessible only to children with r/w privileges
|
||
110 | r->sharedMemID = shmget(key, sizeof(RobotShared), IPC_CREAT | 0666); |
||
111 | 1039 | bneuman | |
112 | 1072 | bneuman | |
113 | 1039 | bneuman | //hack!! TODO: remove!!
|
114 | r->pose.x=first_available_id*100;
|
||
115 | r->pose.y=first_available_id*50;
|
||
116 | |||
117 | |||
118 | 988 | bcoltin | if(r->sharedMemID < 0) |
119 | { |
||
120 | fprintf(stderr, "Failed to get shared memory.\n");
|
||
121 | return -1; |
||
122 | } |
||
123 | |||
124 | r->shared = (RobotShared*)shmat(r->sharedMemID, NULL, 0); |
||
125 | 1072 | bneuman | |
126 | 988 | bcoltin | if(!(r->shared))
|
127 | 1072 | bneuman | { |
128 | 993 | ayeager | //Free shared memory region
|
129 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
130 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
131 | 1072 | bneuman | |
132 | 988 | bcoltin | fprintf(stderr, "Error attaching memory to parent.\n");
|
133 | return -1; |
||
134 | } |
||
135 | 1072 | bneuman | |
136 | 988 | bcoltin | // Initialize robot structure here
|
137 | r->shared->motor1 = 0;
|
||
138 | r->shared->motor2 = 0;
|
||
139 | r->id = id; |
||
140 | |||
141 | 1068 | ayeager | if(execname != NULL) |
142 | 988 | bcoltin | { |
143 | 1068 | ayeager | if((pid = fork()) < 0) |
144 | { |
||
145 | //Free Shared Memory Region
|
||
146 | if (!shmdt(r->shared))
|
||
147 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
148 | 1072 | bneuman | |
149 | 1068 | ayeager | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
150 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
151 | 993 | ayeager | |
152 | 1068 | ayeager | r->id = -1;
|
153 | fprintf(stderr, "Failed to fork robot process.\n");
|
||
154 | return -1; |
||
155 | } |
||
156 | 997 | bcoltin | } |
157 | 988 | bcoltin | |
158 | 1072 | bneuman | |
159 | 1068 | ayeager | if(!pid && execname)
|
160 | 988 | bcoltin | { |
161 | 996 | bneuman | char var[21]; |
162 | /* restore default sigchld handler */
|
||
163 | signal(SIGCHLD, SIG_DFL); |
||
164 | sprintf(var, "memory_id=%d", r->sharedMemID);
|
||
165 | putenv(var); |
||
166 | //TODO: keep the other env. stuff around
|
||
167 | execv(execname, NULL);
|
||
168 | 1039 | bneuman | err(errno, "exec failed to run child process.\n");
|
169 | 988 | bcoltin | } |
170 | else
|
||
171 | { |
||
172 | r->pid = pid; |
||
173 | do {
|
||
174 | first_available_id++; |
||
175 | } while (first_available_id < robots_size &&
|
||
176 | 1006 | bcoltin | robots[first_available_id].id != -1);
|
177 | 988 | bcoltin | // resize the array if necessary
|
178 | if (first_available_id >= robots_size)
|
||
179 | { |
||
180 | robots_size *= 2;
|
||
181 | robots = realloc(robots, sizeof(Robot) * robots_size);
|
||
182 | if (!robots)
|
||
183 | { |
||
184 | fprintf(stderr, "Out of memory.\n");
|
||
185 | return -1; |
||
186 | } |
||
187 | 1039 | bneuman | for (i = robots_size=first_available_id; i < robots_size; i++)
|
188 | robots[i].id = -1;
|
||
189 | |||
190 | 988 | bcoltin | } |
191 | 1072 | bneuman | } |
192 | 1068 | ayeager | |
193 | //Log creation
|
||
194 | if(execname != NULL) |
||
195 | { |
||
196 | //printf("commiting %d\n", num_robots);
|
||
197 | commit(r, num_robots, CREATE_STEP); |
||
198 | 988 | bcoltin | } |
199 | 1068 | ayeager | num_robots++; |
200 | 1006 | bcoltin | |
201 | 988 | bcoltin | return id;
|
202 | } |
||
203 | |||
204 | /**
|
||
205 | * Frees all resources associated with a robot, including
|
||
206 | * killing its process if necessary.
|
||
207 | *
|
||
208 | * @return zero on success, nonzero on failure
|
||
209 | **/
|
||
210 | int robot_destroy(int id) |
||
211 | { |
||
212 | Robot* r; |
||
213 | |||
214 | if (id < 0 || id >= robots_size) |
||
215 | return -1; |
||
216 | r = &robots[id]; |
||
217 | if (r->id == 0) |
||
218 | return -1; |
||
219 | |||
220 | 993 | ayeager | if (!shmdt(r->shared))
|
221 | 988 | bcoltin | { |
222 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
223 | return -1; |
||
224 | } |
||
225 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
226 | { |
||
227 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
228 | return -1; |
||
229 | } |
||
230 | |||
231 | memset(r, 0, sizeof(Robot)); |
||
232 | 997 | bcoltin | r->id = -1;
|
233 | 988 | bcoltin | |
234 | if (id < first_available_id)
|
||
235 | first_available_id = 0;
|
||
236 | 1072 | bneuman | |
237 | |||
238 | 988 | bcoltin | return 0; |
239 | } |
||
240 | 1072 | bneuman | |
241 | 1068 | ayeager | void robot_destroy_all()
|
242 | { |
||
243 | int i;
|
||
244 | for(i = 0; i < num_robots; i++) |
||
245 | robot_destroy(i); |
||
246 | } |
||
247 | 988 | bcoltin | |
248 | 997 | bcoltin | void robot_iterator_reset(void) |
249 | { |
||
250 | 1009 | bcoltin | iterator_pos = -1;
|
251 | 997 | bcoltin | } |
252 | |||
253 | // note: addresses may change, replace with
|
||
254 | // allocated memory
|
||
255 | Robot* robot_iterator_next(void)
|
||
256 | { |
||
257 | 1006 | bcoltin | if (iterator_pos >= robots_size)
|
258 | return NULL; |
||
259 | do iterator_pos++;
|
||
260 | 1072 | bneuman | while (iterator_pos < robots_size &&
|
261 | 1006 | bcoltin | robots[iterator_pos].id == -1);
|
262 | 1072 | bneuman | |
263 | 997 | bcoltin | if (iterator_pos >= robots_size)
|
264 | return NULL; |
||
265 | 1072 | bneuman | |
266 | 997 | bcoltin | return &(robots[iterator_pos]);
|
267 | } |
||
268 | |||
269 | 988 | bcoltin | void sig_chld_handler(int sig) |
270 | { |
||
271 | 1039 | bneuman | int ret,stat;
|
272 | |||
273 | pthread_mutex_lock(&all_finished_mutex); |
||
274 | |||
275 | while((ret = waitpid(-1, &stat, WUNTRACED | WNOHANG))) |
||
276 | { |
||
277 | if(ret==-1) |
||
278 | err(errno,"error waiting on robot");
|
||
279 | |||
280 | if(WIFSTOPPED(stat))
|
||
281 | finished++; |
||
282 | } |
||
283 | |||
284 | if (finished >= num_robots)
|
||
285 | { |
||
286 | pthread_cond_signal(&all_finished_cond); |
||
287 | } |
||
288 | pthread_mutex_unlock(&all_finished_mutex); |
||
289 | 1006 | bcoltin | } |
290 | 988 | bcoltin | |
291 | 1006 | bcoltin | void* robot_event_loop(void* arg) |
292 | { |
||
293 | int i;
|
||
294 | 1063 | bneuman | int ret;
|
295 | 988 | bcoltin | |
296 | 1063 | bneuman | //TODO: not exit if there is an error?
|
297 | |||
298 | 1006 | bcoltin | while (1) |
299 | { |
||
300 | 1076 | bcoltin | //sleep to avoid 100% CPU usage
|
301 | usleep(10000);
|
||
302 | while (pausing)
|
||
303 | { |
||
304 | paused = 1;
|
||
305 | usleep(50000);
|
||
306 | } |
||
307 | paused = 0;
|
||
308 | |||
309 | 1063 | bneuman | ret = pthread_mutex_lock(&all_finished_mutex); |
310 | if(ret)
|
||
311 | err(errno, "error locking mutex in even loop");
|
||
312 | 1006 | bcoltin | // TODO: race condition for adding robots?
|
313 | if (finished < num_robots)
|
||
314 | { |
||
315 | 1063 | bneuman | ret = pthread_cond_wait(&all_finished_cond, &all_finished_mutex); |
316 | if(ret)
|
||
317 | err(errno, "error waiting on condition variable");
|
||
318 | 1006 | bcoltin | } |
319 | finished = 0;
|
||
320 | 1074 | bcoltin | |
321 | ret = pthread_mutex_unlock(&all_finished_mutex); |
||
322 | 1063 | bneuman | |
323 | if(ret)
|
||
324 | fprintf(stderr, "hmmm, error unlocking. errno: %d", errno);
|
||
325 | |||
326 | 1068 | ayeager | timeStep++; |
327 | 1006 | bcoltin | for (i = 0; i < robots_size; i++) |
328 | 1010 | bcoltin | if (robots[i].id != -1) |
329 | 1068 | ayeager | { |
330 | 1010 | bcoltin | robot_update(i); |
331 | 1063 | bneuman | |
332 | 1072 | bneuman | commit(&robots[i],i,timeStep); |
333 | 1068 | ayeager | } |
334 | 1010 | bcoltin | for (i = 0; i < robots_size; i++) |
335 | 1006 | bcoltin | { |
336 | if (robots[i].id == -1) |
||
337 | continue;
|
||
338 | 1063 | bneuman | ret = kill(robots[i].pid, SIGCONT); |
339 | if(ret)
|
||
340 | warn("error: could not kill resume robot proc.");
|
||
341 | 1006 | bcoltin | } |
342 | 988 | bcoltin | |
343 | 1074 | bcoltin | if (timeStep % 5 == 0) |
344 | gui_refresh(); |
||
345 | 1006 | bcoltin | } |
346 | 988 | bcoltin | } |
347 | |||
348 | 1068 | ayeager | void* logger_step_loop(void* arg) |
349 | { |
||
350 | printf("going into logger\n");
|
||
351 | int timeStep;
|
||
352 | int index;
|
||
353 | |||
354 | int prevStep = -1; |
||
355 | 1072 | bneuman | |
356 | 1068 | ayeager | if(fread(&timeStep, sizeof(int), 1, file) < 1) |
357 | 1072 | bneuman | { |
358 | 1068 | ayeager | robot_destroy_all(); |
359 | fprintf(stderr, "problem reading timestep\n");
|
||
360 | 1072 | bneuman | |
361 | 1068 | ayeager | return (void*)-1; |
362 | } |
||
363 | 1072 | bneuman | |
364 | while(1){ |
||
365 | 1068 | ayeager | prevStep = timeStep; |
366 | do
|
||
367 | { |
||
368 | if(fread(&index, sizeof(int), 1, file) < 1) |
||
369 | { |
||
370 | robot_destroy_all(); |
||
371 | return (void*) -1; |
||
372 | } |
||
373 | 1072 | bneuman | |
374 | 1068 | ayeager | //Update the robot
|
375 | if(timeStep == CREATE_STEP)
|
||
376 | { |
||
377 | 1072 | bneuman | robot_create(NULL);
|
378 | 1068 | ayeager | } |
379 | |||
380 | 1072 | bneuman | |
381 | 1068 | ayeager | if(getLog(&robots[index]) < 0) |
382 | { |
||
383 | robot_destroy_all(); |
||
384 | return (void*) -1; |
||
385 | } |
||
386 | |||
387 | //Read in next logfile and check number
|
||
388 | if(fread(&timeStep, sizeof(int), 1, file) < 1) |
||
389 | { |
||
390 | robot_destroy_all(); |
||
391 | return (void*)-1; |
||
392 | } |
||
393 | 1072 | bneuman | |
394 | 1068 | ayeager | if (prevStep < 0) |
395 | prevStep = timeStep; |
||
396 | }while(timeStep == prevStep || timeStep == CREATE_STEP);
|
||
397 | 1072 | bneuman | |
398 | |||
399 | 1068 | ayeager | gui_refresh(); |
400 | 1072 | bneuman | |
401 | 1068 | ayeager | usleep(100);
|
402 | } |
||
403 | 1072 | bneuman | |
404 | 1068 | ayeager | } |
405 | |||
406 | 1010 | bcoltin | void robot_update(int i) |
407 | { |
||
408 | Robot* r = &(robots[i]); |
||
409 | if (r->id == -1) |
||
410 | return;
|
||
411 | move_robot(r); |
||
412 | 1044 | bpoole | update_rangefinders(r); |
413 | 1010 | bcoltin | } |