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