root / branches / simulator / projects / simulator / simulator / core / robot.c @ 1006
History | View | Annotate | Download (4.84 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 | 988 | bcoltin | |
25 | #include "robot.h" |
||
26 | 1006 | bcoltin | #include "gtk_gui.h" |
27 | 988 | bcoltin | |
28 | void sig_chld_handler(int sig); |
||
29 | |||
30 | // global variables
|
||
31 | int first_available_id = 0; |
||
32 | // an expanding array of robots
|
||
33 | Robot* robots = NULL;
|
||
34 | int robots_size = 0; |
||
35 | 1006 | bcoltin | int num_robots = 0; |
36 | 988 | bcoltin | |
37 | 997 | bcoltin | int iterator_pos = 0; |
38 | |||
39 | 1006 | bcoltin | // signalled when all robots have run for a bit
|
40 | pthread_mutex_t all_finished_mutex; |
||
41 | pthread_cond_t all_finished_cond; |
||
42 | int finished = 0; |
||
43 | |||
44 | 988 | bcoltin | int robots_initialize(void) |
45 | { |
||
46 | 997 | bcoltin | int i;
|
47 | 988 | bcoltin | if (signal(SIGCHLD, sig_chld_handler) == SIG_ERR)
|
48 | return -1; |
||
49 | |||
50 | robots_size = 10;
|
||
51 | robots = (Robot*)malloc(sizeof(Robot) * robots_size);
|
||
52 | if (!robots)
|
||
53 | return -1; |
||
54 | memset(robots, 0, sizeof(Robot) * robots_size); |
||
55 | 997 | bcoltin | for (i = 0; i < robots_size; i++) |
56 | robots[i].id = -1;
|
||
57 | 1006 | bcoltin | |
58 | finished = 0;
|
||
59 | pthread_mutex_init(&all_finished_mutex, NULL);
|
||
60 | pthread_cond_init(&all_finished_cond, NULL);
|
||
61 | 988 | bcoltin | |
62 | return 0; |
||
63 | } |
||
64 | |||
65 | /**
|
||
66 | * Creates a new robot. Returns its id on
|
||
67 | * success, or a negative integer on failure.
|
||
68 | **/
|
||
69 | int robot_create(char *execname) |
||
70 | { |
||
71 | int pid;
|
||
72 | int id = first_available_id;
|
||
73 | Robot* r = &robots[id]; |
||
74 | 1006 | bcoltin | printf("ID: %d\n", id);
|
75 | 988 | bcoltin | // do shared memory stuff here
|
76 | key_t key = IPC_PRIVATE; |
||
77 | //Memory accessible only to children with r/w privileges
|
||
78 | r->sharedMemID = shmget(key, sizeof(RobotShared), IPC_CREAT | 0666); |
||
79 | |||
80 | if(r->sharedMemID < 0) |
||
81 | { |
||
82 | fprintf(stderr, "Failed to get shared memory.\n");
|
||
83 | return -1; |
||
84 | } |
||
85 | |||
86 | r->shared = (RobotShared*)shmat(r->sharedMemID, NULL, 0); |
||
87 | |||
88 | if(!(r->shared))
|
||
89 | 993 | ayeager | { |
90 | //Free shared memory region
|
||
91 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
92 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
93 | |||
94 | 988 | bcoltin | fprintf(stderr, "Error attaching memory to parent.\n");
|
95 | return -1; |
||
96 | } |
||
97 | |||
98 | // Initialize robot structure here
|
||
99 | r->shared->motor1 = 0;
|
||
100 | r->shared->motor2 = 0;
|
||
101 | r->id = id; |
||
102 | |||
103 | if((pid = fork()) < 0) |
||
104 | { |
||
105 | 993 | ayeager | //Free Shared Memory Region
|
106 | 997 | bcoltin | if (!shmdt(r->shared))
|
107 | 993 | ayeager | fprintf(stderr, "Failed to free shared memory.\n");
|
108 | |||
109 | 997 | bcoltin | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
110 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
111 | 993 | ayeager | |
112 | 997 | bcoltin | r->id = -1;
|
113 | 988 | bcoltin | fprintf(stderr, "Failed to fork robot process.\n");
|
114 | return -1; |
||
115 | 997 | bcoltin | } |
116 | 988 | bcoltin | |
117 | |||
118 | if(!pid)
|
||
119 | { |
||
120 | 996 | bneuman | char var[21]; |
121 | /* restore default sigchld handler */
|
||
122 | signal(SIGCHLD, SIG_DFL); |
||
123 | sprintf(var, "memory_id=%d", r->sharedMemID);
|
||
124 | putenv(var); |
||
125 | //TODO: keep the other env. stuff around
|
||
126 | execv(execname, NULL);
|
||
127 | fprintf(stderr, "exec failed to run child process.\n");
|
||
128 | exit(-1);
|
||
129 | 988 | bcoltin | } |
130 | else
|
||
131 | { |
||
132 | r->pid = pid; |
||
133 | do {
|
||
134 | first_available_id++; |
||
135 | } while (first_available_id < robots_size &&
|
||
136 | 1006 | bcoltin | robots[first_available_id].id != -1);
|
137 | 988 | bcoltin | // resize the array if necessary
|
138 | if (first_available_id >= robots_size)
|
||
139 | { |
||
140 | robots_size *= 2;
|
||
141 | robots = realloc(robots, sizeof(Robot) * robots_size);
|
||
142 | if (!robots)
|
||
143 | { |
||
144 | fprintf(stderr, "Out of memory.\n");
|
||
145 | return -1; |
||
146 | } |
||
147 | } |
||
148 | } |
||
149 | 1006 | bcoltin | |
150 | num_robots++; |
||
151 | 988 | bcoltin | |
152 | return id;
|
||
153 | } |
||
154 | |||
155 | /**
|
||
156 | * Frees all resources associated with a robot, including
|
||
157 | * killing its process if necessary.
|
||
158 | *
|
||
159 | * @return zero on success, nonzero on failure
|
||
160 | **/
|
||
161 | int robot_destroy(int id) |
||
162 | { |
||
163 | Robot* r; |
||
164 | |||
165 | if (id < 0 || id >= robots_size) |
||
166 | return -1; |
||
167 | r = &robots[id]; |
||
168 | if (r->id == 0) |
||
169 | return -1; |
||
170 | |||
171 | 993 | ayeager | if (!shmdt(r->shared))
|
172 | 988 | bcoltin | { |
173 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
174 | return -1; |
||
175 | } |
||
176 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
177 | { |
||
178 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
179 | return -1; |
||
180 | } |
||
181 | |||
182 | memset(r, 0, sizeof(Robot)); |
||
183 | 997 | bcoltin | r->id = -1;
|
184 | 988 | bcoltin | |
185 | if (id < first_available_id)
|
||
186 | first_available_id = 0;
|
||
187 | |||
188 | return 0; |
||
189 | } |
||
190 | |||
191 | 997 | bcoltin | void robot_iterator_reset(void) |
192 | { |
||
193 | iterator_pos = 0;
|
||
194 | } |
||
195 | |||
196 | // note: addresses may change, replace with
|
||
197 | // allocated memory
|
||
198 | Robot* robot_iterator_next(void)
|
||
199 | { |
||
200 | 1006 | bcoltin | if (iterator_pos >= robots_size)
|
201 | return NULL; |
||
202 | do iterator_pos++;
|
||
203 | 997 | bcoltin | while (iterator_pos < robots_size &&
|
204 | 1006 | bcoltin | robots[iterator_pos].id == -1);
|
205 | 997 | bcoltin | |
206 | if (iterator_pos >= robots_size)
|
||
207 | return NULL; |
||
208 | |||
209 | return &(robots[iterator_pos]);
|
||
210 | } |
||
211 | |||
212 | 988 | bcoltin | void sig_chld_handler(int sig) |
213 | { |
||
214 | 1006 | bcoltin | pthread_mutex_lock(&all_finished_mutex); |
215 | finished++; |
||
216 | if (finished >= num_robots)
|
||
217 | { |
||
218 | pthread_cond_signal(&all_finished_cond); |
||
219 | } |
||
220 | pthread_mutex_unlock(&all_finished_mutex); |
||
221 | } |
||
222 | 988 | bcoltin | |
223 | 1006 | bcoltin | void* robot_event_loop(void* arg) |
224 | { |
||
225 | int i;
|
||
226 | //TODO: errors
|
||
227 | 988 | bcoltin | |
228 | 1006 | bcoltin | while (1) |
229 | { |
||
230 | pthread_mutex_lock(&all_finished_mutex); |
||
231 | // TODO: race condition for adding robots?
|
||
232 | if (finished < num_robots)
|
||
233 | { |
||
234 | pthread_cond_wait(&all_finished_cond, &all_finished_mutex); |
||
235 | } |
||
236 | finished = 0;
|
||
237 | pthread_mutex_unlock(&all_finished_mutex); |
||
238 | for (i = 0; i < robots_size; i++) |
||
239 | { |
||
240 | if (robots[i].id == -1) |
||
241 | continue;
|
||
242 | kill(robots[i].pid, SIGCONT); |
||
243 | } |
||
244 | 988 | bcoltin | |
245 | 1006 | bcoltin | gui_refresh(); |
246 | } |
||
247 | 988 | bcoltin | } |