root / branches / simulator / projects / simulator / simulator / core / robot.c @ 993
History | View | Annotate | Download (3.61 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 | |||
24 | #include "robot.h" |
||
25 | |||
26 | void sig_chld_handler(int sig); |
||
27 | |||
28 | // global variables
|
||
29 | int first_available_id = 0; |
||
30 | // an expanding array of robots
|
||
31 | Robot* robots = NULL;
|
||
32 | int robots_size = 0; |
||
33 | |||
34 | int robots_initialize(void) |
||
35 | { |
||
36 | if (signal(SIGCHLD, sig_chld_handler) == SIG_ERR)
|
||
37 | return -1; |
||
38 | |||
39 | robots_size = 10;
|
||
40 | robots = (Robot*)malloc(sizeof(Robot) * robots_size);
|
||
41 | if (!robots)
|
||
42 | return -1; |
||
43 | memset(robots, 0, sizeof(Robot) * robots_size); |
||
44 | |||
45 | return 0; |
||
46 | } |
||
47 | |||
48 | /**
|
||
49 | * Creates a new robot. Returns its id on
|
||
50 | * success, or a negative integer on failure.
|
||
51 | **/
|
||
52 | int robot_create(char *execname) |
||
53 | { |
||
54 | int pid;
|
||
55 | int id = first_available_id;
|
||
56 | Robot* r = &robots[id]; |
||
57 | // do shared memory stuff here
|
||
58 | key_t key = IPC_PRIVATE; |
||
59 | //Memory accessible only to children with r/w privileges
|
||
60 | r->sharedMemID = shmget(key, sizeof(RobotShared), IPC_CREAT | 0666); |
||
61 | |||
62 | if(r->sharedMemID < 0) |
||
63 | { |
||
64 | fprintf(stderr, "Failed to get shared memory.\n");
|
||
65 | return -1; |
||
66 | } |
||
67 | |||
68 | r->shared = (RobotShared*)shmat(r->sharedMemID, NULL, 0); |
||
69 | |||
70 | if(!(r->shared))
|
||
71 | 993 | ayeager | { |
72 | //Free shared memory region
|
||
73 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
74 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
75 | |||
76 | 988 | bcoltin | fprintf(stderr, "Error attaching memory to parent.\n");
|
77 | return -1; |
||
78 | } |
||
79 | |||
80 | // Initialize robot structure here
|
||
81 | r->shared->motor1 = 0;
|
||
82 | r->shared->motor2 = 0;
|
||
83 | r->id = id; |
||
84 | |||
85 | if((pid = fork()) < 0) |
||
86 | { |
||
87 | 993 | ayeager | |
88 | //Free Shared Memory Region
|
||
89 | if (!shmdt(r->shared))
|
||
90 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
91 | |||
92 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
93 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
94 | |||
95 | 988 | bcoltin | r->id = 0;
|
96 | fprintf(stderr, "Failed to fork robot process.\n");
|
||
97 | return -1; |
||
98 | } |
||
99 | |||
100 | |||
101 | if(!pid)
|
||
102 | { |
||
103 | char var[21]; |
||
104 | char *envp[] = {var, NULL}; |
||
105 | /* restore default sigchld handler */
|
||
106 | signal(SIGCHLD, SIG_DFL); |
||
107 | sprintf(var, "memory_id=%d", r->sharedMemID);
|
||
108 | //TODO: keep the other env. stuff around
|
||
109 | execve(execname, NULL, envp);
|
||
110 | fprintf(stderr, "exec failed to run child process.\n");
|
||
111 | exit(-1);
|
||
112 | } |
||
113 | else
|
||
114 | { |
||
115 | r->pid = pid; |
||
116 | do {
|
||
117 | first_available_id++; |
||
118 | } while (first_available_id < robots_size &&
|
||
119 | robots[first_available_id].id != 0);
|
||
120 | // resize the array if necessary
|
||
121 | if (first_available_id >= robots_size)
|
||
122 | { |
||
123 | robots_size *= 2;
|
||
124 | robots = realloc(robots, sizeof(Robot) * robots_size);
|
||
125 | if (!robots)
|
||
126 | { |
||
127 | fprintf(stderr, "Out of memory.\n");
|
||
128 | return -1; |
||
129 | } |
||
130 | } |
||
131 | } |
||
132 | |||
133 | return id;
|
||
134 | } |
||
135 | |||
136 | /**
|
||
137 | * Frees all resources associated with a robot, including
|
||
138 | * killing its process if necessary.
|
||
139 | *
|
||
140 | * @return zero on success, nonzero on failure
|
||
141 | **/
|
||
142 | int robot_destroy(int id) |
||
143 | { |
||
144 | Robot* r; |
||
145 | |||
146 | if (id < 0 || id >= robots_size) |
||
147 | return -1; |
||
148 | r = &robots[id]; |
||
149 | if (r->id == 0) |
||
150 | return -1; |
||
151 | |||
152 | 993 | ayeager | if (!shmdt(r->shared))
|
153 | 988 | bcoltin | { |
154 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
155 | return -1; |
||
156 | } |
||
157 | if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
||
158 | { |
||
159 | fprintf(stderr, "Failed to free shared memory.\n");
|
||
160 | return -1; |
||
161 | } |
||
162 | |||
163 | memset(r, 0, sizeof(Robot)); |
||
164 | |||
165 | if (id < first_available_id)
|
||
166 | first_available_id = 0;
|
||
167 | |||
168 | return 0; |
||
169 | } |
||
170 | |||
171 | void sig_chld_handler(int sig) |
||
172 | { |
||
173 | pid_t pid; |
||
174 | int cstat;
|
||
175 | |||
176 | printf("SIGCHLD: parent waiting\n");
|
||
177 | |||
178 | pid = waitpid(0, &cstat, WUNTRACED);
|
||
179 | |||
180 | printf("got %d from %d\n", cstat, pid);
|
||
181 | |||
182 | kill(pid, SIGCONT); |
||
183 | } |