Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (7.2 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 robots_initialize(void)
57
{
58
        int i;
59
        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
        for (i = 0; i < robots_size; i++)
68
                robots[i].id = -1;
69
        
70
        finished = 0;
71
        pthread_mutex_init(&all_finished_mutex, NULL);
72
        pthread_cond_init(&all_finished_cond, NULL);
73

    
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
          int pid,i;
84
        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

    
92
        //hack!! TODO: remove!!
93
        r->pose.x=first_available_id*100;
94
        r->pose.y=first_available_id*50;
95

    
96

    
97
        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
        {        
107
                //Free shared memory region
108
                if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
109
                        fprintf(stderr, "Failed to free shared memory.\n");
110
                       
111
                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
        if(execname != NULL)
121
        {
122
                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

    
131
                        r->id = -1;
132
                        fprintf(stderr, "Failed to fork robot process.\n");
133
                        return -1;
134
                }
135
        }
136
        
137

    
138
        if(!pid && execname)
139
        {
140
          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
          err(errno, "exec failed to run child process.\n");
148
        }
149
        else
150
        {
151
                r->pid = pid;
152
                do {
153
                        first_available_id++;
154
                } while (first_available_id < robots_size &&
155
                                robots[first_available_id].id != -1);
156
                // 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
                        for (i = robots_size=first_available_id; i < robots_size; i++)
167
                          robots[i].id = -1;
168

    
169
                }
170
        }      
171

    
172
        //Log creation
173
        if(execname != NULL)
174
        {
175
                //printf("commiting %d\n", num_robots);
176
                commit(r, num_robots, CREATE_STEP);
177
        }
178
        num_robots++;
179

    
180
        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
    if (!shmdt(r->shared))
200
        {
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
        r->id = -1;
212

    
213
        if (id < first_available_id)
214
                first_available_id = 0;
215
        
216
        
217
        return 0;
218
}
219
         
220
void robot_destroy_all()
221
{
222
        int i;
223
        for(i = 0; i < num_robots; i++)
224
                robot_destroy(i);
225
}
226

    
227
void robot_iterator_reset(void)
228
{
229
        iterator_pos = -1;
230
}
231

    
232
// note: addresses may change, replace with
233
// allocated memory
234
Robot* robot_iterator_next(void)
235
{
236
        if (iterator_pos >= robots_size)
237
                return NULL;
238
        do iterator_pos++;
239
        while (iterator_pos < robots_size && 
240
                        robots[iterator_pos].id == -1);
241
        
242
        if (iterator_pos >= robots_size)
243
                return NULL;
244
        
245
        return &(robots[iterator_pos]);
246
}
247

    
248
void sig_chld_handler(int sig)
249
{
250
  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
}
269

    
270
void* robot_event_loop(void* arg)
271
{
272
        int i;
273
        int ret;
274

    
275
        //TODO: not exit if there is an error?
276

    
277
        while (1)
278
        {
279
                ret = pthread_mutex_lock(&all_finished_mutex);
280
                if(ret)
281
                  err(errno, "error locking mutex in even loop");
282
                // TODO: race condition for adding robots?
283
                if (finished < num_robots)
284
                {
285
                        ret = pthread_cond_wait(&all_finished_cond, &all_finished_mutex);
286
                        if(ret)
287
                          err(errno, "error waiting on condition variable");
288
                }
289
                finished = 0;
290

    
291
                ret = pthread_mutex_unlock(&all_finished_mutex);
292
                if(ret)
293
                  fprintf(stderr, "hmmm, error unlocking. errno: %d", errno);
294

    
295
                timeStep++;
296
                for (i = 0; i < robots_size; i++)
297
                        if (robots[i].id != -1)
298
                        {
299
                                robot_update(i);
300

    
301
                                printf("commit %d\n", i);
302
                                commit(&robots[i],i,timeStep); 
303
                        }
304
                for (i = 0; i < robots_size; i++)
305
                {
306
                        if (robots[i].id == -1)
307
                                continue;
308
                        ret = kill(robots[i].pid, SIGCONT);
309
                        if(ret)
310
                          warn("error: could not kill resume robot proc.");
311
                }
312

    
313
                gui_refresh();
314
        }
315
}
316

    
317
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
void robot_update(int i)
376
{
377
        Robot* r = &(robots[i]);
378
        if (r->id == -1)
379
                return;
380
        move_robot(r);
381
        update_rangefinders(r);
382
}
383