Revision 1068
Added replay functionality and command line arguments
-l <filename> log simulation to file [currently make sure file does not
exist, overwriting of files not handled well]
-p <filename> run a log file rather than simulation
-w <filename> load in world file
| robot.c | ||
|---|---|---|
| 22 | 22 |
#include <errno.h> |
| 23 | 23 |
#include <pthread.h> |
| 24 | 24 |
#include <err.h> |
| 25 |
#include <unistd.h> |
|
| 25 | 26 |
|
| 26 | 27 |
#include "gtk_gui.h" |
| 27 | 28 |
|
| ... | ... | |
| 29 | 30 |
#include "motion.h" |
| 30 | 31 |
#include "rangefinders.h" |
| 31 | 32 |
#include "world.h" |
| 33 |
#include "logger.h" |
|
| 34 |
#include "gtk_gui.h" |
|
| 32 | 35 |
|
| 36 |
|
|
| 33 | 37 |
void sig_chld_handler(int sig); |
| 34 | 38 |
void robot_update(int i); |
| 35 | 39 |
|
| ... | ... | |
| 40 | 44 |
int robots_size = 0; |
| 41 | 45 |
int num_robots = 0; |
| 42 | 46 |
|
| 43 |
int iterator_pos = 0; |
|
| 47 |
int iterator_pos = 0;
|
|
| 44 | 48 |
|
| 49 |
int timeStep = 0; |
|
| 50 |
|
|
| 45 | 51 |
// signalled when all robots have run for a bit |
| 46 | 52 |
pthread_mutex_t all_finished_mutex; |
| 47 | 53 |
pthread_cond_t all_finished_cond; |
| ... | ... | |
| 74 | 80 |
**/ |
| 75 | 81 |
int robot_create(char *execname) |
| 76 | 82 |
{
|
| 77 |
int pid,i; |
|
| 83 |
int pid,i;
|
|
| 78 | 84 |
int id = first_available_id; |
| 79 | 85 |
Robot* r = &robots[id]; |
| 80 | 86 |
// do shared memory stuff here |
| ... | ... | |
| 111 | 117 |
r->shared->motor2 = 0; |
| 112 | 118 |
r->id = id; |
| 113 | 119 |
|
| 114 |
if((pid = fork()) < 0)
|
|
| 120 |
if(execname != NULL)
|
|
| 115 | 121 |
{
|
| 116 |
//Free Shared Memory Region |
|
| 117 |
if (!shmdt(r->shared)) |
|
| 118 |
fprintf(stderr, "Failed to free shared memory.\n"); |
|
| 119 |
|
|
| 120 |
if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
|
| 121 |
fprintf(stderr, "Failed to free shared memory.\n"); |
|
| 122 |
if((pid = fork()) < 0) |
|
| 123 |
{
|
|
| 124 |
//Free Shared Memory Region |
|
| 125 |
if (!shmdt(r->shared)) |
|
| 126 |
fprintf(stderr, "Failed to free shared memory.\n"); |
|
| 127 |
|
|
| 128 |
if (!shmctl(r->sharedMemID, IPC_RMID, NULL)) |
|
| 129 |
fprintf(stderr, "Failed to free shared memory.\n"); |
|
| 122 | 130 |
|
| 123 |
r->id = -1; |
|
| 124 |
fprintf(stderr, "Failed to fork robot process.\n"); |
|
| 125 |
return -1; |
|
| 131 |
r->id = -1; |
|
| 132 |
fprintf(stderr, "Failed to fork robot process.\n"); |
|
| 133 |
return -1; |
|
| 134 |
} |
|
| 126 | 135 |
} |
| 127 | 136 |
|
| 128 | 137 |
|
| 129 |
if(!pid) |
|
| 138 |
if(!pid && execname)
|
|
| 130 | 139 |
{
|
| 131 | 140 |
char var[21]; |
| 132 | 141 |
/* restore default sigchld handler */ |
| ... | ... | |
| 158 | 167 |
robots[i].id = -1; |
| 159 | 168 |
|
| 160 | 169 |
} |
| 170 |
} |
|
| 171 |
|
|
| 172 |
//Log creation |
|
| 173 |
if(execname != NULL) |
|
| 174 |
{
|
|
| 175 |
//printf("commiting %d\n", num_robots);
|
|
| 176 |
commit(r, num_robots, CREATE_STEP); |
|
| 161 | 177 |
} |
| 178 |
num_robots++; |
|
| 162 | 179 |
|
| 163 |
num_robots++; |
|
| 164 |
|
|
| 165 | 180 |
return id; |
| 166 | 181 |
} |
| 167 | 182 |
|
| ... | ... | |
| 198 | 213 |
if (id < first_available_id) |
| 199 | 214 |
first_available_id = 0; |
| 200 | 215 |
|
| 216 |
|
|
| 201 | 217 |
return 0; |
| 202 | 218 |
} |
| 219 |
|
|
| 220 |
void robot_destroy_all() |
|
| 221 |
{
|
|
| 222 |
int i; |
|
| 223 |
for(i = 0; i < num_robots; i++) |
|
| 224 |
robot_destroy(i); |
|
| 225 |
} |
|
| 203 | 226 |
|
| 204 | 227 |
void robot_iterator_reset(void) |
| 205 | 228 |
{
|
| ... | ... | |
| 269 | 292 |
if(ret) |
| 270 | 293 |
fprintf(stderr, "hmmm, error unlocking. errno: %d", errno); |
| 271 | 294 |
|
| 295 |
timeStep++; |
|
| 272 | 296 |
for (i = 0; i < robots_size; i++) |
| 273 | 297 |
if (robots[i].id != -1) |
| 298 |
{
|
|
| 274 | 299 |
robot_update(i); |
| 275 | 300 |
|
| 301 |
printf("commit %d\n", i);
|
|
| 302 |
commit(&robots[i],i,timeStep); |
|
| 303 |
} |
|
| 276 | 304 |
for (i = 0; i < robots_size; i++) |
| 277 | 305 |
{
|
| 278 | 306 |
if (robots[i].id == -1) |
| ... | ... | |
| 286 | 314 |
} |
| 287 | 315 |
} |
| 288 | 316 |
|
| 317 |
void* logger_step_loop(void* arg) |
|
| 318 |
{
|
|
| 319 |
printf("going into logger\n");
|
|
| 320 |
int timeStep; |
|
| 321 |
int index; |
|
| 322 |
|
|
| 323 |
int prevStep = -1; |
|
| 324 |
|
|
| 325 |
if(fread(&timeStep, sizeof(int), 1, file) < 1) |
|
| 326 |
{
|
|
| 327 |
robot_destroy_all(); |
|
| 328 |
fprintf(stderr, "problem reading timestep\n"); |
|
| 329 |
|
|
| 330 |
return (void*)-1; |
|
| 331 |
} |
|
| 332 |
|
|
| 333 |
while(1){
|
|
| 334 |
prevStep = timeStep; |
|
| 335 |
do |
|
| 336 |
{
|
|
| 337 |
if(fread(&index, sizeof(int), 1, file) < 1) |
|
| 338 |
{
|
|
| 339 |
robot_destroy_all(); |
|
| 340 |
return (void*) -1; |
|
| 341 |
} |
|
| 342 |
|
|
| 343 |
//Update the robot |
|
| 344 |
if(timeStep == CREATE_STEP) |
|
| 345 |
{
|
|
| 346 |
robot_create(NULL); |
|
| 347 |
} |
|
| 348 |
|
|
| 349 |
|
|
| 350 |
if(getLog(&robots[index]) < 0) |
|
| 351 |
{
|
|
| 352 |
robot_destroy_all(); |
|
| 353 |
return (void*) -1; |
|
| 354 |
} |
|
| 355 |
|
|
| 356 |
//Read in next logfile and check number |
|
| 357 |
if(fread(&timeStep, sizeof(int), 1, file) < 1) |
|
| 358 |
{
|
|
| 359 |
robot_destroy_all(); |
|
| 360 |
return (void*)-1; |
|
| 361 |
} |
|
| 362 |
|
|
| 363 |
if (prevStep < 0) |
|
| 364 |
prevStep = timeStep; |
|
| 365 |
}while(timeStep == prevStep || timeStep == CREATE_STEP); |
|
| 366 |
|
|
| 367 |
|
|
| 368 |
gui_refresh(); |
|
| 369 |
|
|
| 370 |
usleep(100); |
|
| 371 |
} |
|
| 372 |
|
|
| 373 |
} |
|
| 374 |
|
|
| 289 | 375 |
void robot_update(int i) |
| 290 | 376 |
{
|
| 291 | 377 |
Robot* r = &(robots[i]); |
Also available in: Unified diff