root / branches / simulator / projects / simulator / simulator / core / robot.c @ 1044
History | View | Annotate | Download (5.47 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 | 1039 | bneuman | #include <err.h> |
25 | 988 | bcoltin | |
26 | #include "robot.h" |
||
27 | 1010 | bcoltin | #include "motion.h" |
28 | 1044 | bpoole | #include "world.h" |
29 | 1006 | bcoltin | #include "gtk_gui.h" |
30 | 988 | bcoltin | |
31 | void sig_chld_handler(int sig); |
||
32 | 1010 | bcoltin | void robot_update(int i); |
33 | 988 | bcoltin | |
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 | 1006 | bcoltin | int num_robots = 0; |
40 | 988 | bcoltin | |
41 | 997 | bcoltin | int iterator_pos = 0; |
42 | |||
43 | 1006 | bcoltin | // 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 | 988 | bcoltin | int robots_initialize(void) |
49 | { |
||
50 | 997 | bcoltin | int i;
|
51 | 988 | bcoltin | 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 | 997 | bcoltin | for (i = 0; i < robots_size; i++) |
60 | robots[i].id = -1;
|
||
61 | 1006 | bcoltin | |
62 | finished = 0;
|
||
63 | pthread_mutex_init(&all_finished_mutex, NULL);
|
||
64 | pthread_cond_init(&all_finished_cond, NULL);
|
||
65 | 988 | bcoltin | |
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 | 1039 | bneuman | int pid,i;
|
76 | 988 | bcoltin | int id = first_available_id;
|
77 | Robot* r = &robots[id]; |
||
78 | 1006 | bcoltin | printf("ID: %d\n", id);
|
79 | 988 | bcoltin | // 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 | 1039 | bneuman | |
85 | //hack!! TODO: remove!!
|
||
86 | r->pose.x=first_available_id*100;
|
||
87 | r->pose.y=first_available_id*50;
|
||
88 | |||
89 | |||
90 | 988 | bcoltin | 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 | 993 | ayeager | { |
100 | //Free shared memory region
|
||
101 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
102 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
103 | |||
104 | 988 | bcoltin | 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 | 993 | ayeager | //Free Shared Memory Region
|
116 | 997 | bcoltin | if (!shmdt(r->shared))
|
117 | 993 | ayeager | fprintf(stderr, "Failed to free shared memory.\n");
|
118 | |||
119 | 997 | bcoltin | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
120 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
121 | 993 | ayeager | |
122 | 997 | bcoltin | r->id = -1;
|
123 | 988 | bcoltin | fprintf(stderr, "Failed to fork robot process.\n");
|
124 | return -1; |
||
125 | 997 | bcoltin | } |
126 | 988 | bcoltin | |
127 | |||
128 | if(!pid)
|
||
129 | { |
||
130 | 996 | bneuman | 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 | 1039 | bneuman | err(errno, "exec failed to run child process.\n");
|
138 | 988 | bcoltin | } |
139 | else
|
||
140 | { |
||
141 | r->pid = pid; |
||
142 | do {
|
||
143 | first_available_id++; |
||
144 | } while (first_available_id < robots_size &&
|
||
145 | 1006 | bcoltin | robots[first_available_id].id != -1);
|
146 | 988 | bcoltin | // 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 | 1039 | bneuman | for (i = robots_size=first_available_id; i < robots_size; i++)
|
157 | robots[i].id = -1;
|
||
158 | |||
159 | 988 | bcoltin | } |
160 | } |
||
161 | 1006 | bcoltin | |
162 | num_robots++; |
||
163 | 988 | bcoltin | |
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 | 993 | ayeager | if (!shmdt(r->shared))
|
184 | 988 | bcoltin | { |
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 | 997 | bcoltin | r->id = -1;
|
196 | 988 | bcoltin | |
197 | if (id < first_available_id)
|
||
198 | first_available_id = 0;
|
||
199 | |||
200 | return 0; |
||
201 | } |
||
202 | |||
203 | 997 | bcoltin | void robot_iterator_reset(void) |
204 | { |
||
205 | 1009 | bcoltin | iterator_pos = -1;
|
206 | 997 | bcoltin | } |
207 | |||
208 | // note: addresses may change, replace with
|
||
209 | // allocated memory
|
||
210 | Robot* robot_iterator_next(void)
|
||
211 | { |
||
212 | 1006 | bcoltin | if (iterator_pos >= robots_size)
|
213 | return NULL; |
||
214 | do iterator_pos++;
|
||
215 | 997 | bcoltin | while (iterator_pos < robots_size &&
|
216 | 1006 | bcoltin | robots[iterator_pos].id == -1);
|
217 | 997 | bcoltin | |
218 | if (iterator_pos >= robots_size)
|
||
219 | return NULL; |
||
220 | |||
221 | return &(robots[iterator_pos]);
|
||
222 | } |
||
223 | |||
224 | 988 | bcoltin | void sig_chld_handler(int sig) |
225 | { |
||
226 | 1039 | bneuman | 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 | 1006 | bcoltin | } |
245 | 988 | bcoltin | |
246 | 1006 | bcoltin | void* robot_event_loop(void* arg) |
247 | { |
||
248 | int i;
|
||
249 | //TODO: errors
|
||
250 | 988 | bcoltin | |
251 | 1006 | bcoltin | 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 | 1010 | bcoltin | if (robots[i].id != -1) |
263 | robot_update(i); |
||
264 | for (i = 0; i < robots_size; i++) |
||
265 | 1006 | bcoltin | { |
266 | if (robots[i].id == -1) |
||
267 | continue;
|
||
268 | kill(robots[i].pid, SIGCONT); |
||
269 | } |
||
270 | 988 | bcoltin | |
271 | 1006 | bcoltin | gui_refresh(); |
272 | } |
||
273 | 988 | bcoltin | } |
274 | |||
275 | 1010 | bcoltin | void robot_update(int i) |
276 | { |
||
277 | Robot* r = &(robots[i]); |
||
278 | if (r->id == -1) |
||
279 | return;
|
||
280 | move_robot(r); |
||
281 | 1044 | bpoole | update_rangefinders(r); |
282 | 1010 | bcoltin | } |