Project

General

Profile

Statistics
| Revision:

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

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

    
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
        {        
72
                //Free shared memory region
73
                if (!shmctl(r->sharedMemID, IPC_RMID, NULL))
74
                        fprintf(stderr, "Failed to free shared memory.\n");
75
                       
76
                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
                
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
                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
    if (!shmdt(r->shared))
153
        {
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
}
184