root / branches / simulator / projects / simulator / simulator / core / robot.c @ 1047
History | View | Annotate | Download (5.45 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 | // do shared memory stuff here
|
||
79 | key_t key = IPC_PRIVATE; |
||
80 | //Memory accessible only to children with r/w privileges
|
||
81 | r->sharedMemID = shmget(key, sizeof(RobotShared), IPC_CREAT | 0666); |
||
82 | |||
83 | 1039 | bneuman | |
84 | //hack!! TODO: remove!!
|
||
85 | r->pose.x=first_available_id*100;
|
||
86 | r->pose.y=first_available_id*50;
|
||
87 | |||
88 | |||
89 | 988 | bcoltin | if(r->sharedMemID < 0) |
90 | { |
||
91 | fprintf(stderr, "Failed to get shared memory.\n");
|
||
92 | return -1; |
||
93 | } |
||
94 | |||
95 | r->shared = (RobotShared*)shmat(r->sharedMemID, NULL, 0); |
||
96 | |||
97 | if(!(r->shared))
|
||
98 | 993 | ayeager | { |
99 | //Free shared memory region
|
||
100 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
101 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
102 | |||
103 | 988 | bcoltin | fprintf(stderr, "Error attaching memory to parent.\n");
|
104 | return -1; |
||
105 | } |
||
106 | |||
107 | // Initialize robot structure here
|
||
108 | r->shared->motor1 = 0;
|
||
109 | r->shared->motor2 = 0;
|
||
110 | r->id = id; |
||
111 | |||
112 | if((pid = fork()) < 0) |
||
113 | { |
||
114 | 993 | ayeager | //Free Shared Memory Region
|
115 | 997 | bcoltin | if (!shmdt(r->shared))
|
116 | 993 | ayeager | fprintf(stderr, "Failed to free shared memory.\n");
|
117 | |||
118 | 997 | bcoltin | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
119 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
120 | 993 | ayeager | |
121 | 997 | bcoltin | r->id = -1;
|
122 | 988 | bcoltin | fprintf(stderr, "Failed to fork robot process.\n");
|
123 | return -1; |
||
124 | 997 | bcoltin | } |
125 | 988 | bcoltin | |
126 | |||
127 | if(!pid)
|
||
128 | { |
||
129 | 996 | bneuman | char var[21]; |
130 | /* restore default sigchld handler */
|
||
131 | signal(SIGCHLD, SIG_DFL); |
||
132 | sprintf(var, "memory_id=%d", r->sharedMemID);
|
||
133 | putenv(var); |
||
134 | //TODO: keep the other env. stuff around
|
||
135 | execv(execname, NULL);
|
||
136 | 1039 | bneuman | err(errno, "exec failed to run child process.\n");
|
137 | 988 | bcoltin | } |
138 | else
|
||
139 | { |
||
140 | r->pid = pid; |
||
141 | do {
|
||
142 | first_available_id++; |
||
143 | } while (first_available_id < robots_size &&
|
||
144 | 1006 | bcoltin | robots[first_available_id].id != -1);
|
145 | 988 | bcoltin | // resize the array if necessary
|
146 | if (first_available_id >= robots_size)
|
||
147 | { |
||
148 | robots_size *= 2;
|
||
149 | robots = realloc(robots, sizeof(Robot) * robots_size);
|
||
150 | if (!robots)
|
||
151 | { |
||
152 | fprintf(stderr, "Out of memory.\n");
|
||
153 | return -1; |
||
154 | } |
||
155 | 1039 | bneuman | for (i = robots_size=first_available_id; i < robots_size; i++)
|
156 | robots[i].id = -1;
|
||
157 | |||
158 | 988 | bcoltin | } |
159 | } |
||
160 | 1006 | bcoltin | |
161 | num_robots++; |
||
162 | 988 | bcoltin | |
163 | return id;
|
||
164 | } |
||
165 | |||
166 | /**
|
||
167 | * Frees all resources associated with a robot, including
|
||
168 | * killing its process if necessary.
|
||
169 | *
|
||
170 | * @return zero on success, nonzero on failure
|
||
171 | **/
|
||
172 | int robot_destroy(int id) |
||
173 | { |
||
174 | Robot* r; |
||
175 | |||
176 | if (id < 0 || id >= robots_size) |
||
177 | return -1; |
||
178 | r = &robots[id]; |
||
179 | if (r->id == 0) |
||
180 | return -1; |
||
181 | |||
182 | 993 | ayeager | if (!shmdt(r->shared))
|
183 | 988 | bcoltin | { |
184 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
185 | return -1; |
||
186 | } |
||
187 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
188 | { |
||
189 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
190 | return -1; |
||
191 | } |
||
192 | |||
193 | memset(r, 0, sizeof(Robot)); |
||
194 | 997 | bcoltin | r->id = -1;
|
195 | 988 | bcoltin | |
196 | if (id < first_available_id)
|
||
197 | first_available_id = 0;
|
||
198 | |||
199 | return 0; |
||
200 | } |
||
201 | |||
202 | 997 | bcoltin | void robot_iterator_reset(void) |
203 | { |
||
204 | 1009 | bcoltin | iterator_pos = -1;
|
205 | 997 | bcoltin | } |
206 | |||
207 | // note: addresses may change, replace with
|
||
208 | // allocated memory
|
||
209 | Robot* robot_iterator_next(void)
|
||
210 | { |
||
211 | 1006 | bcoltin | if (iterator_pos >= robots_size)
|
212 | return NULL; |
||
213 | do iterator_pos++;
|
||
214 | 997 | bcoltin | while (iterator_pos < robots_size &&
|
215 | 1006 | bcoltin | robots[iterator_pos].id == -1);
|
216 | 997 | bcoltin | |
217 | if (iterator_pos >= robots_size)
|
||
218 | return NULL; |
||
219 | |||
220 | return &(robots[iterator_pos]);
|
||
221 | } |
||
222 | |||
223 | 988 | bcoltin | void sig_chld_handler(int sig) |
224 | { |
||
225 | 1039 | bneuman | int ret,stat;
|
226 | |||
227 | pthread_mutex_lock(&all_finished_mutex); |
||
228 | |||
229 | while((ret = waitpid(-1, &stat, WUNTRACED | WNOHANG))) |
||
230 | { |
||
231 | if(ret==-1) |
||
232 | err(errno,"error waiting on robot");
|
||
233 | |||
234 | if(WIFSTOPPED(stat))
|
||
235 | finished++; |
||
236 | } |
||
237 | |||
238 | if (finished >= num_robots)
|
||
239 | { |
||
240 | pthread_cond_signal(&all_finished_cond); |
||
241 | } |
||
242 | pthread_mutex_unlock(&all_finished_mutex); |
||
243 | 1006 | bcoltin | } |
244 | 988 | bcoltin | |
245 | 1006 | bcoltin | void* robot_event_loop(void* arg) |
246 | { |
||
247 | int i;
|
||
248 | //TODO: errors
|
||
249 | 988 | bcoltin | |
250 | 1006 | bcoltin | while (1) |
251 | { |
||
252 | pthread_mutex_lock(&all_finished_mutex); |
||
253 | // TODO: race condition for adding robots?
|
||
254 | if (finished < num_robots)
|
||
255 | { |
||
256 | pthread_cond_wait(&all_finished_cond, &all_finished_mutex); |
||
257 | } |
||
258 | finished = 0;
|
||
259 | pthread_mutex_unlock(&all_finished_mutex); |
||
260 | for (i = 0; i < robots_size; i++) |
||
261 | 1010 | bcoltin | if (robots[i].id != -1) |
262 | robot_update(i); |
||
263 | for (i = 0; i < robots_size; i++) |
||
264 | 1006 | bcoltin | { |
265 | if (robots[i].id == -1) |
||
266 | continue;
|
||
267 | kill(robots[i].pid, SIGCONT); |
||
268 | } |
||
269 | 988 | bcoltin | |
270 | 1006 | bcoltin | gui_refresh(); |
271 | } |
||
272 | 988 | bcoltin | } |
273 | |||
274 | 1010 | bcoltin | void robot_update(int i) |
275 | { |
||
276 | Robot* r = &(robots[i]); |
||
277 | if (r->id == -1) |
||
278 | return;
|
||
279 | move_robot(r); |
||
280 | 1044 | bpoole | update_rangefinders(r); |
281 | 1010 | bcoltin | } |