Project

General

Profile

Statistics
| Revision:

root / branches / simulator / projects / simulator / simulator / core / robot.c @ 1077

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 1077 bcoltin
                        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 1072 bneuman
                                commit(&robots[i],i,timeStep);
332 1068 ayeager
                        }
333 1010 bcoltin
                for (i = 0; i < robots_size; i++)
334 1006 bcoltin
                {
335
                        if (robots[i].id == -1)
336
                                continue;
337 1063 bneuman
                        ret = kill(robots[i].pid, SIGCONT);
338
                        if(ret)
339
                          warn("error: could not kill resume robot proc.");
340 1006 bcoltin
                }
341 988 bcoltin
342 1074 bcoltin
                if (timeStep % 5 == 0)
343
                        gui_refresh();
344 1006 bcoltin
        }
345 988 bcoltin
}
346
347 1068 ayeager
void* logger_step_loop(void* arg)
348
{
349
        printf("going into logger\n");
350
        int timeStep;
351
        int index;
352
353
        int prevStep = -1;
354 1072 bneuman
355 1068 ayeager
        if(fread(&timeStep, sizeof(int), 1, file) < 1)
356 1072 bneuman
        {
357 1068 ayeager
                robot_destroy_all();
358
                fprintf(stderr, "problem reading timestep\n");
359 1072 bneuman
360 1068 ayeager
            return (void*)-1;
361
        }
362 1072 bneuman
363
         while(1){
364 1068 ayeager
                prevStep = timeStep;
365
                do
366
                   {
367
                        if(fread(&index, sizeof(int), 1, file) < 1)
368
                        {
369
                                robot_destroy_all();
370
                                 return (void*) -1;
371
                        }
372 1072 bneuman
373 1068 ayeager
            //Update the robot
374
                        if(timeStep == CREATE_STEP)
375
                        {
376 1072 bneuman
                            robot_create(NULL);
377 1068 ayeager
                        }
378
379 1072 bneuman
380 1068 ayeager
                        if(getLog(&robots[index]) < 0)
381
                        {
382
                                robot_destroy_all();
383
                                 return (void*) -1;
384
                        }
385
386
                        //Read in next logfile and check number
387
                           if(fread(&timeStep, sizeof(int), 1, file) < 1)
388
                        {
389
                                robot_destroy_all();
390
                                return (void*)-1;
391
                        }
392 1072 bneuman
393 1068 ayeager
                   if (prevStep < 0)
394
                                   prevStep = timeStep;
395
                   }while(timeStep == prevStep || timeStep == CREATE_STEP);
396 1072 bneuman
397
398 1068 ayeager
                gui_refresh();
399 1072 bneuman
400 1068 ayeager
                usleep(100);
401
        }
402 1072 bneuman
403 1068 ayeager
}
404
405 1010 bcoltin
void robot_update(int i)
406
{
407
        Robot* r = &(robots[i]);
408
        if (r->id == -1)
409
                return;
410
        move_robot(r);
411 1044 bpoole
        update_rangefinders(r);
412 1010 bcoltin
}