Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (8.55 KB)

1
/**
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
#include <pthread.h>
24
#include <err.h>
25
#include <unistd.h>
26

    
27
#include "gtk_gui.h"
28

    
29
#include "robot.h"
30
#include "motion.h"
31
#include "rangefinders.h"
32
#include "world.h"
33
#include "logger.h"
34
#include "gtk_gui.h"
35

    
36

    
37
void sig_chld_handler(int sig);
38
void robot_update(int i);
39

    
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
int num_robots = 0;
46

    
47
int iterator_pos = 0;
48

    
49
int timeStep = 0;
50

    
51
// 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
int pausing = 0;
57
int paused = 0;
58

    
59
int robots_initialize(void)
60
{
61
        int i;
62
        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
        for (i = 0; i < robots_size; i++)
71
                robots[i].id = -1;
72

    
73
        finished = 0;
74
        pthread_mutex_init(&all_finished_mutex, NULL);
75
        pthread_cond_init(&all_finished_cond, NULL);
76

    
77
        return 0;
78
}
79

    
80
int robots_cleanup(void)
81
{
82
  int i,ret;
83

    
84
  //wait for all robots to be suspended to avoid race conditions
85
  //TODO: what if this never comes...
86
  ret = pthread_cond_wait(&all_finished_cond, &all_finished_mutex);
87
  if(ret)
88
    err(errno, "error waiting on condition variable");
89

    
90
  //clear the sigchild handler
91
  signal(SIGCHLD, SIG_DFL);
92

    
93
  //destroy the robots
94
  for(i=0; i<num_robots; i++)
95
    robot_destroy(i);
96

    
97
  free(robots);
98

    
99
  return 0;
100
}
101

    
102
void robots_pause(void)
103
{
104
        pausing++;
105
        while (!paused) usleep(10000);
106
}
107

    
108
void robots_resume(void)
109
{
110
        pausing--;
111
        if (pausing < 0)
112
                fprintf(stderr, "More resuming than pausing....\n");
113
}
114

    
115
/**
116
 * Creates a new robot. Returns its id on
117
 * success, or a negative integer on failure.
118
 **/
119
int robot_create(char *execname)
120
{
121
          int pid,i;
122
        int id = first_available_id;
123
        Robot* r = &robots[id];
124
        // do shared memory stuff here
125
        key_t key = IPC_PRIVATE;
126

    
127

    
128
        /* colony simulator executable must end in .csim */
129
        if(strstr(execname, ".csim") - execname != strlen(execname) - 5)
130
        {
131
          fprintf(stderr, "Error, \"%s\" is not recognized as a colony simulator executable\n", execname);
132
          return -1;
133
        }
134

    
135
        //Memory accessible only to children with r/w privileges
136
        r->sharedMemID = shmget(key, sizeof(RobotShared), IPC_CREAT | 0666);
137

    
138

    
139
        //hack!! TODO: remove!!
140
        r->pose.x=first_available_id*100;
141
        r->pose.y=first_available_id*50;
142
        r->pose.theta = 0.5;
143

    
144

    
145
        if(r->sharedMemID < 0)
146
        {
147
                fprintf(stderr, "Failed to get shared memory.\n");
148
                return -1;
149
        }
150

    
151
        r->shared = (RobotShared*)shmat(r->sharedMemID, NULL, 0);
152

    
153
        if(!(r->shared))
154
        {
155
                //Free shared memory region
156
                if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
157
                        fprintf(stderr, "Failed to free shared memory.\n");
158

    
159
                fprintf(stderr, "Error attaching memory to parent.\n");
160
                return -1;
161
        }
162

    
163
        // Initialize robot structure here
164
        r->shared->motor1 = 0;
165
        r->shared->motor2 = 0;
166
        r->id = id;
167

    
168
        if(execname != NULL)
169
        {
170
                if((pid = fork()) < 0)
171
                {
172
                        //Free Shared Memory Region
173
                        //TODO: this doesn't work
174
                        if (!shmdt(r->shared))
175
                                fprintf(stderr, "Failed to free shared memory.\n");
176

    
177
                        if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
178
                                fprintf(stderr, "Failed to free shared memory.\n");
179

    
180
                        r->id = -1;
181
                        fprintf(stderr, "Failed to fork robot process.\n");
182
                        return -1;
183
                }
184
        }
185

    
186

    
187
        if(!pid && execname)
188
        {
189
          char var[21];
190
          /* restore default sigchld handler */
191
          signal(SIGCHLD, SIG_DFL);
192
          sprintf(var, "memory_id=%d", r->sharedMemID);
193
          putenv(var);
194
          //TODO: keep the other env. stuff around
195
          execv(execname, NULL);
196
          err(errno, "exec failed to run child process.\n");
197
        }
198
        else
199
        {
200
                r->pid = pid;
201
                do {
202
                        first_available_id++;
203
                } while (first_available_id < robots_size &&
204
                                robots[first_available_id].id != -1);
205
                // resize the array if necessary
206
                if (first_available_id >= robots_size)
207
                {
208
                        robots_size *= 2;
209
                        robots = realloc(robots, sizeof(Robot) * robots_size);
210
                        if (!robots)
211
                        {
212
                                fprintf(stderr, "Out of memory.\n");
213
                                return -1;
214
                        }
215
                        for (i = robots_size=first_available_id; i < robots_size; i++)
216
                          robots[i].id = -1;
217

    
218
                }
219
        }
220

    
221
        //Log creation
222
        if(execname != NULL)
223
        {
224
                //printf("commiting %d\n", num_robots);
225
                commit(r, num_robots, CREATE_STEP);
226
        }
227
        num_robots++;
228

    
229
        return id;
230
}
231

    
232
/**
233
 * Frees all resources associated with a robot, including
234
 * killing its process if necessary.
235
 *
236
 * @return zero on success, nonzero on failure
237
 **/
238
int robot_destroy(int id)
239
{
240
        Robot* r;
241

    
242

    
243
        if (id < 0 || id >= robots_size)
244
                return -1;
245
        r = &robots[id];
246
        if (r->id == -1)
247
                return -1;
248
        
249
        // TODO: this doesn't work
250
        if (shmdt(r->shared))
251
        {
252
          warn("Failed to free shared memory.");
253
          return -1;
254
        }
255
        //mark the shared meory to be destroyed
256
        if (shmctl(r->sharedMemID, IPC_RMID, NULL))
257
        {
258
                warn("Failed to mark shared memory for deletion\n");
259
                return -1;
260
        }
261
        //TODO: send a signal that the robot can catch to detach its shared mem
262
        if (kill(r->pid, SIGKILL))
263
        {
264
                warn( "Failed to kill robot process.\n");
265
                return -1;
266
        }
267

    
268
        memset(r, 0, sizeof(Robot));
269
        r->id = -1;
270

    
271

    
272
        if (id < first_available_id)
273
                first_available_id = 0;
274

    
275

    
276
        return 0;
277
}
278

    
279
void robot_destroy_all()
280
{
281
        int i;
282
        for(i = 0; i < num_robots; i++)
283
                robot_destroy(i);
284
}
285

    
286
void robot_iterator_reset(void)
287
{
288
        iterator_pos = -1;
289
}
290

    
291
// note: addresses may change, replace with
292
// allocated memory
293
Robot* robot_iterator_next(void)
294
{
295
        if (iterator_pos >= robots_size)
296
                return NULL;
297
        do iterator_pos++;
298
        while (iterator_pos < robots_size &&
299
                        robots[iterator_pos].id == -1);
300

    
301
        if (iterator_pos >= robots_size)
302
                return NULL;
303

    
304
        return &(robots[iterator_pos]);
305
}
306

    
307
Robot* robot_get(int id)
308
{
309
        if (id >= 0 && id < robots_size)
310
                return &(robots[id]);
311
        else
312
                return NULL;
313
}
314

    
315
void sig_chld_handler(int sig)
316
{
317
  int ret,stat,i;
318

    
319
  pthread_mutex_lock(&all_finished_mutex);
320

    
321

    
322
  for(i=0; i<num_robots; i++) {
323

    
324
    ret = waitpid(robots[i].pid, &stat, WUNTRACED | WNOHANG);
325
  
326
    if(ret==-1)
327
      err(errno,"error waiting on robot");
328

    
329
    if(WIFSTOPPED(stat))
330
      finished++;
331
  }
332

    
333
  if (finished >= num_robots)
334
  {
335
    pthread_cond_signal(&all_finished_cond);
336
  }
337
  pthread_mutex_unlock(&all_finished_mutex);
338
}
339

    
340
void* robot_event_loop(void* arg)
341
{
342
        int i;
343
        int ret;
344

    
345
        //TODO: not exit if there is an error?
346

    
347
        while (1)
348
        {
349
                //sleep to avoid 100% CPU usage
350
                usleep(10000);
351
                while (pausing)
352
                {
353
                        paused = 1;
354
                        usleep(50000);
355
                }
356
                paused = 0;
357

    
358
                ret = pthread_mutex_lock(&all_finished_mutex);
359
                if(ret)
360
                        err(errno, "error locking mutex in even loop");
361
                // TODO: race condition for adding robots?
362
                if (finished < num_robots)
363
                {
364
                        ret = pthread_cond_wait(&all_finished_cond, &all_finished_mutex);
365
                        if(ret)
366
                          err(errno, "error waiting on condition variable");
367
                }
368
                finished = 0;
369
                
370
                ret = pthread_mutex_unlock(&all_finished_mutex);
371

    
372
                if(ret)
373
                  fprintf(stderr, "hmmm, error unlocking. errno: %d", errno);
374

    
375
                timeStep++;
376
                for (i = 0; i < robots_size; i++)
377
                        if (robots[i].id != -1)
378
                        {
379
                                robot_update(i);
380
                                commit(&robots[i],i,timeStep);
381
                        }
382
                for (i = 0; i < robots_size; i++)
383
                {
384
                        if (robots[i].id == -1)
385
                                continue;
386
                        ret = kill(robots[i].pid, SIGCONT);
387
                        if(ret)
388
                          warn("error: could not kill resume robot proc.");
389
                }
390

    
391
                if (timeStep % 5 == 0)
392
                        gui_refresh();
393
        }
394
}
395

    
396
void* logger_step_loop(void* arg)
397
{
398
        printf("going into logger\n");
399
        int timeStep;
400
        int index;
401

    
402
        int prevStep = -1;
403

    
404
        if(fread(&timeStep, sizeof(int), 1, file) < 1)
405
        {
406
                robot_destroy_all();
407
                fprintf(stderr, "problem reading timestep\n");
408

    
409
            return (void*)-1;
410
        }
411

    
412
         while(1){
413
                prevStep = timeStep;
414
                do
415
                   {
416
                        if(fread(&index, sizeof(int), 1, file) < 1)
417
                        {
418
                                robot_destroy_all();
419
                                 return (void*) -1;
420
                        }
421

    
422
            //Update the robot
423
                        if(timeStep == CREATE_STEP)
424
                        {
425
                            robot_create(NULL);
426
                        }
427

    
428

    
429
                        if(getLog(&robots[index]) < 0)
430
                        {
431
                                robot_destroy_all();
432
                                 return (void*) -1;
433
                        }
434

    
435
                        //Read in next logfile and check number
436
                           if(fread(&timeStep, sizeof(int), 1, file) < 1)
437
                        {
438
                                robot_destroy_all();
439
                                return (void*)-1;
440
                        }
441

    
442
                   if (prevStep < 0)
443
                                   prevStep = timeStep;
444
                   }while(timeStep == prevStep || timeStep == CREATE_STEP);
445

    
446

    
447
                gui_refresh();
448

    
449
                usleep(100);
450
        }
451

    
452
}
453

    
454
void robot_update(int i)
455
{
456
        Robot* r = &(robots[i]);
457
        if (r->id == -1)
458
                return;
459
        move_robot(r);
460
        update_rangefinders(r);
461
}
462