Statistics
| Revision:

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

History | View | Annotate | Download (7.8 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
        robots_pause();
83
        //TODO: clean things up
84
        return 0;
85
}
86

    
87
void robots_pause(void)
88
{
89
        pausing++;
90
        while (!paused) usleep(10000);
91
}
92

    
93
void robots_resume(void)
94
{
95
        pausing--;
96
        if (pausing < 0)
97
                fprintf(stderr, "More resuming than pausing....\n");
98
}
99

    
100
/**
101
 * Creates a new robot. Returns its id on
102
 * success, or a negative integer on failure.
103
 **/
104
int robot_create(char *execname)
105
{
106
          int pid,i;
107
        int id = first_available_id;
108
        Robot* r = &robots[id];
109
        // do shared memory stuff here
110
        key_t key = IPC_PRIVATE;
111
        //Memory accessible only to children with r/w privileges
112
        r->sharedMemID = shmget(key, sizeof(RobotShared), IPC_CREAT | 0666);
113

    
114

    
115
        //hack!! TODO: remove!!
116
        r->pose.x=first_available_id*100;
117
        r->pose.y=first_available_id*50;
118

    
119

    
120
        if(r->sharedMemID < 0)
121
        {
122
                fprintf(stderr, "Failed to get shared memory.\n");
123
                return -1;
124
        }
125

    
126
        r->shared = (RobotShared*)shmat(r->sharedMemID, NULL, 0);
127

    
128
        if(!(r->shared))
129
        {
130
                //Free shared memory region
131
                if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
132
                        fprintf(stderr, "Failed to free shared memory.\n");
133

    
134
                fprintf(stderr, "Error attaching memory to parent.\n");
135
                return -1;
136
        }
137

    
138
        // Initialize robot structure here
139
        r->shared->motor1 = 0;
140
        r->shared->motor2 = 0;
141
        r->id = id;
142

    
143
        if(execname != NULL)
144
        {
145
                if((pid = fork()) < 0)
146
                {
147
                        //Free Shared Memory Region
148
                        //TODO: this doesn't work
149
                        if (!shmdt(r->shared))
150
                                fprintf(stderr, "Failed to free shared memory.\n");
151

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

    
155
                        r->id = -1;
156
                        fprintf(stderr, "Failed to fork robot process.\n");
157
                        return -1;
158
                }
159
        }
160

    
161

    
162
        if(!pid && execname)
163
        {
164
          char var[21];
165
          /* restore default sigchld handler */
166
          signal(SIGCHLD, SIG_DFL);
167
          sprintf(var, "memory_id=%d", r->sharedMemID);
168
          putenv(var);
169
          //TODO: keep the other env. stuff around
170
          execv(execname, NULL);
171
          err(errno, "exec failed to run child process.\n");
172
        }
173
        else
174
        {
175
                r->pid = pid;
176
                do {
177
                        first_available_id++;
178
                } while (first_available_id < robots_size &&
179
                                robots[first_available_id].id != -1);
180
                // resize the array if necessary
181
                if (first_available_id >= robots_size)
182
                {
183
                        robots_size *= 2;
184
                        robots = realloc(robots, sizeof(Robot) * robots_size);
185
                        if (!robots)
186
                        {
187
                                fprintf(stderr, "Out of memory.\n");
188
                                return -1;
189
                        }
190
                        for (i = robots_size=first_available_id; i < robots_size; i++)
191
                          robots[i].id = -1;
192

    
193
                }
194
        }
195

    
196
        //Log creation
197
        if(execname != NULL)
198
        {
199
                //printf("commiting %d\n", num_robots);
200
                commit(r, num_robots, CREATE_STEP);
201
        }
202
        num_robots++;
203

    
204
        return id;
205
}
206

    
207
/**
208
 * Frees all resources associated with a robot, including
209
 * killing its process if necessary.
210
 *
211
 * @return zero on success, nonzero on failure
212
 **/
213
int robot_destroy(int id)
214
{
215
        Robot* r;
216

    
217
        if (id < 0 || id >= robots_size)
218
                return -1;
219
        r = &robots[id];
220
        if (r->id == -1)
221
                return -1;
222
        
223
        // TODO: this doesn't work
224
    if (!shmdt(r->shared))
225
        {
226
                fprintf(stderr, "Failed to free shared memory.\n");
227
                //return -1;
228
        }
229
        if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
230
        {
231
                fprintf(stderr, "Failed to free shared memory.\n");
232
                //return -1;
233
        }
234
        if (kill(r->pid, SIGKILL))
235
        {
236
                fprintf(stderr, "Failed to kill robot process.\n");
237
                return -1;
238
        }
239

    
240
        memset(r, 0, sizeof(Robot));
241
        r->id = -1;
242

    
243
        if (id < first_available_id)
244
                first_available_id = 0;
245

    
246

    
247
        return 0;
248
}
249

    
250
void robot_destroy_all()
251
{
252
        int i;
253
        for(i = 0; i < num_robots; i++)
254
                robot_destroy(i);
255
}
256

    
257
void robot_iterator_reset(void)
258
{
259
        iterator_pos = -1;
260
}
261

    
262
// note: addresses may change, replace with
263
// allocated memory
264
Robot* robot_iterator_next(void)
265
{
266
        if (iterator_pos >= robots_size)
267
                return NULL;
268
        do iterator_pos++;
269
        while (iterator_pos < robots_size &&
270
                        robots[iterator_pos].id == -1);
271

    
272
        if (iterator_pos >= robots_size)
273
                return NULL;
274

    
275
        return &(robots[iterator_pos]);
276
}
277

    
278
Robot* robot_get(int id)
279
{
280
        if (id >= 0 && id < robots_size)
281
                return &(robots[id]);
282
        else
283
                return NULL;
284
}
285

    
286
void sig_chld_handler(int sig)
287
{
288
  int ret,stat;
289

    
290
  pthread_mutex_lock(&all_finished_mutex);
291

    
292
  while((ret = waitpid(-1, &stat, WUNTRACED | WNOHANG)))
293
  {
294
    if(ret==-1)
295
      err(errno,"error waiting on robot");
296

    
297
    if(WIFSTOPPED(stat))
298
      finished++;
299
  }
300

    
301
  if (finished >= num_robots)
302
  {
303
    pthread_cond_signal(&all_finished_cond);
304
  }
305
  pthread_mutex_unlock(&all_finished_mutex);
306
}
307

    
308
void* robot_event_loop(void* arg)
309
{
310
        int i;
311
        int ret;
312

    
313
        //TODO: not exit if there is an error?
314

    
315
        while (1)
316
        {
317
                //sleep to avoid 100% CPU usage
318
                usleep(10000);
319
                while (pausing)
320
                {
321
                        paused = 1;
322
                        usleep(50000);
323
                }
324
                paused = 0;
325

    
326
                ret = pthread_mutex_lock(&all_finished_mutex);
327
                if(ret)
328
                        err(errno, "error locking mutex in even loop");
329
                // TODO: race condition for adding robots?
330
                if (finished < num_robots)
331
                {
332
                        ret = pthread_cond_wait(&all_finished_cond, &all_finished_mutex);
333
                        if(ret)
334
                          err(errno, "error waiting on condition variable");
335
                }
336
                finished = 0;
337
                
338
                ret = pthread_mutex_unlock(&all_finished_mutex);
339

    
340
                if(ret)
341
                  fprintf(stderr, "hmmm, error unlocking. errno: %d", errno);
342

    
343
                timeStep++;
344
                for (i = 0; i < robots_size; i++)
345
                        if (robots[i].id != -1)
346
                        {
347
                                robot_update(i);
348
                                commit(&robots[i],i,timeStep);
349
                        }
350
                for (i = 0; i < robots_size; i++)
351
                {
352
                        if (robots[i].id == -1)
353
                                continue;
354
                        ret = kill(robots[i].pid, SIGCONT);
355
                        if(ret)
356
                          warn("error: could not kill resume robot proc.");
357
                }
358

    
359
                if (timeStep % 5 == 0)
360
                        gui_refresh();
361
        }
362
}
363

    
364
void* logger_step_loop(void* arg)
365
{
366
        printf("going into logger\n");
367
        int timeStep;
368
        int index;
369

    
370
        int prevStep = -1;
371

    
372
        if(fread(&timeStep, sizeof(int), 1, file) < 1)
373
        {
374
                robot_destroy_all();
375
                fprintf(stderr, "problem reading timestep\n");
376

    
377
            return (void*)-1;
378
        }
379

    
380
         while(1){
381
                prevStep = timeStep;
382
                do
383
                   {
384
                        if(fread(&index, sizeof(int), 1, file) < 1)
385
                        {
386
                                robot_destroy_all();
387
                                 return (void*) -1;
388
                        }
389

    
390
            //Update the robot
391
                        if(timeStep == CREATE_STEP)
392
                        {
393
                            robot_create(NULL);
394
                        }
395

    
396

    
397
                        if(getLog(&robots[index]) < 0)
398
                        {
399
                                robot_destroy_all();
400
                                 return (void*) -1;
401
                        }
402

    
403
                        //Read in next logfile and check number
404
                           if(fread(&timeStep, sizeof(int), 1, file) < 1)
405
                        {
406
                                robot_destroy_all();
407
                                return (void*)-1;
408
                        }
409

    
410
                   if (prevStep < 0)
411
                                   prevStep = timeStep;
412
                   }while(timeStep == prevStep || timeStep == CREATE_STEP);
413

    
414

    
415
                gui_refresh();
416

    
417
                usleep(100);
418
        }
419

    
420
}
421

    
422
void robot_update(int i)
423
{
424
        Robot* r = &(robots[i]);
425
        if (r->id == -1)
426
                return;
427
        move_robot(r);
428
        update_rangefinders(r);
429
}
430