Statistics
| Revision:

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

History | View | Annotate | Download (5.47 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

    
26
#include "robot.h"
27
#include "motion.h"
28
#include "world.h"
29
#include "gtk_gui.h"
30

    
31
void sig_chld_handler(int sig);
32
void robot_update(int i);
33

    
34
// global variables
35
int first_available_id = 0;
36
// an expanding array of robots
37
Robot* robots = NULL;
38
int robots_size = 0;
39
int num_robots = 0;
40

    
41
int iterator_pos = 0;
42

    
43
// signalled when all robots have run for a bit
44
pthread_mutex_t all_finished_mutex;
45
pthread_cond_t all_finished_cond;
46
int finished = 0;
47

    
48
int robots_initialize(void)
49
{
50
        int i;
51
        if (signal(SIGCHLD, sig_chld_handler) == SIG_ERR)
52
                return -1;
53

    
54
        robots_size = 10;
55
        robots = (Robot*)malloc(sizeof(Robot) * robots_size);
56
        if (!robots)
57
                return -1;
58
        memset(robots, 0, sizeof(Robot) * robots_size);
59
        for (i = 0; i < robots_size; i++)
60
                robots[i].id = -1;
61
        
62
        finished = 0;
63
        pthread_mutex_init(&all_finished_mutex, NULL);
64
        pthread_cond_init(&all_finished_cond, NULL);
65

    
66
        return 0;
67
}
68

    
69
/**
70
 * Creates a new robot. Returns its id on
71
 * success, or a negative integer on failure.
72
 **/
73
int robot_create(char *execname)
74
{
75
  int pid,i;
76
        int id = first_available_id;
77
        Robot* r = &robots[id];
78
        printf("ID: %d\n", id);
79
        // do shared memory stuff here
80
        key_t key = IPC_PRIVATE;
81
        //Memory accessible only to children with r/w privileges
82
        r->sharedMemID = shmget(key, sizeof(RobotShared), IPC_CREAT | 0666);
83
        
84

    
85
        //hack!! TODO: remove!!
86
        r->pose.x=first_available_id*100;
87
        r->pose.y=first_available_id*50;
88

    
89

    
90
        if(r->sharedMemID < 0)
91
        {
92
                fprintf(stderr, "Failed to get shared memory.\n");
93
                return -1;
94
        }
95

    
96
        r->shared = (RobotShared*)shmat(r->sharedMemID, NULL, 0);
97
        
98
        if(!(r->shared))
99
        {        
100
                //Free shared memory region
101
                if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
102
                        fprintf(stderr, "Failed to free shared memory.\n");
103
                       
104
                fprintf(stderr, "Error attaching memory to parent.\n");
105
                return -1;
106
        }
107
        
108
        // Initialize robot structure here
109
        r->shared->motor1 = 0;
110
        r->shared->motor2 = 0;
111
        r->id = id;
112

    
113
        if((pid = fork()) < 0)
114
        {
115
                //Free Shared Memory Region
116
                if (!shmdt(r->shared))
117
                        fprintf(stderr, "Failed to free shared memory.\n");
118
                
119
                if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
120
                        fprintf(stderr, "Failed to free shared memory.\n");
121

    
122
                r->id = -1;
123
                fprintf(stderr, "Failed to fork robot process.\n");
124
                return -1;
125
        }
126
        
127

    
128
        if(!pid)
129
        {
130
          char var[21];
131
          /* restore default sigchld handler */
132
          signal(SIGCHLD, SIG_DFL);
133
          sprintf(var, "memory_id=%d", r->sharedMemID);
134
          putenv(var);
135
          //TODO: keep the other env. stuff around
136
          execv(execname, NULL);
137
          err(errno, "exec failed to run child process.\n");
138
        }
139
        else
140
        {
141
                r->pid = pid;
142
                do {
143
                        first_available_id++;
144
                } while (first_available_id < robots_size &&
145
                                robots[first_available_id].id != -1);
146
                // resize the array if necessary
147
                if (first_available_id >= robots_size)
148
                {
149
                        robots_size *= 2;
150
                        robots = realloc(robots, sizeof(Robot) * robots_size);
151
                        if (!robots)
152
                        {
153
                                fprintf(stderr, "Out of memory.\n");
154
                                return -1;
155
                        }
156
                        for (i = robots_size=first_available_id; i < robots_size; i++)
157
                          robots[i].id = -1;
158

    
159
                }
160
        }
161

    
162
        num_robots++;
163
        
164
        return id;
165
}
166

    
167
/**
168
 * Frees all resources associated with a robot, including
169
 * killing its process if necessary.
170
 *
171
 * @return zero on success, nonzero on failure
172
 **/
173
int robot_destroy(int id)
174
{
175
        Robot* r;
176

    
177
        if (id < 0 || id >= robots_size)
178
                return -1;
179
        r = &robots[id];
180
        if (r->id == 0)
181
                return -1;
182

    
183
    if (!shmdt(r->shared))
184
        {
185
                fprintf(stderr, "Failed to free shared memory.\n");
186
                return -1;
187
        }
188
        if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
189
        {
190
                fprintf(stderr, "Failed to free shared memory.\n");
191
                return -1;
192
        }
193

    
194
        memset(r, 0, sizeof(Robot));
195
        r->id = -1;
196

    
197
        if (id < first_available_id)
198
                first_available_id = 0;
199
        
200
        return 0;
201
}
202

    
203
void robot_iterator_reset(void)
204
{
205
        iterator_pos = -1;
206
}
207

    
208
// note: addresses may change, replace with
209
// allocated memory
210
Robot* robot_iterator_next(void)
211
{
212
        if (iterator_pos >= robots_size)
213
                return NULL;
214
        do iterator_pos++;
215
        while (iterator_pos < robots_size && 
216
                        robots[iterator_pos].id == -1);
217
        
218
        if (iterator_pos >= robots_size)
219
                return NULL;
220
        
221
        return &(robots[iterator_pos]);
222
}
223

    
224
void sig_chld_handler(int sig)
225
{
226
  int ret,stat;
227

    
228
  pthread_mutex_lock(&all_finished_mutex);
229

    
230
  while((ret = waitpid(-1, &stat, WUNTRACED | WNOHANG)))
231
  {
232
    if(ret==-1)
233
      err(errno,"error waiting on robot");
234

    
235
    if(WIFSTOPPED(stat))
236
      finished++;
237
  }
238

    
239
  if (finished >= num_robots)
240
  {
241
    pthread_cond_signal(&all_finished_cond);
242
  }
243
  pthread_mutex_unlock(&all_finished_mutex);
244
}
245

    
246
void* robot_event_loop(void* arg)
247
{
248
        int i;
249
        //TODO: errors
250

    
251
        while (1)
252
        {
253
                pthread_mutex_lock(&all_finished_mutex);
254
                // TODO: race condition for adding robots?
255
                if (finished < num_robots)
256
                {
257
                        pthread_cond_wait(&all_finished_cond, &all_finished_mutex);
258
                }
259
                finished = 0;
260
                pthread_mutex_unlock(&all_finished_mutex);
261
                for (i = 0; i < robots_size; i++)
262
                        if (robots[i].id != -1)
263
                                robot_update(i);
264
                for (i = 0; i < robots_size; i++)
265
                {
266
                        if (robots[i].id == -1)
267
                                continue;
268
                        kill(robots[i].pid, SIGCONT);
269
                }
270

    
271
                gui_refresh();
272
        }
273
}
274

    
275
void robot_update(int i)
276
{
277
        Robot* r = &(robots[i]);
278
        if (r->id == -1)
279
                return;
280
        move_robot(r);
281
        update_rangefinders(r);
282
}
283