Colony

/**

 * @file robot.c

 * @author Colony Project

 * @brief Simulator Robot Management

 *

 * Contains implementations of functions managing robots

 * in the simulator, including process control and memory

 * management.

 **/

#include <stdlib.h>

#include <stdio.h>

#include <string.h>

#include <unistd.h>

#include <signal.h>

#include <sys/wait.h>

#include <robot_shared.h>

#include <sys/ipc.h>

#include <sys/types.h>

#include <sys/shm.h>

#include <errno.h>

#include <pthread.h>

#include <err.h>

#include <unistd.h>

#include "gtk_gui.h"

#include "robot.h"

#include "motion.h"

#include "rangefinders.h"

#include "world.h"

#include "logger.h"

#include "gtk_gui.h"

void sig_chld_handler(int sig);

void robot_update(int i);

// global variables

int first_available_id = 0;

// an expanding array of robots

Robot* robots = NULL;

int robots_size = 0;

int num_robots = 0;

int iterator_pos = 0;

int timeStep = 0;

// signalled when all robots have run for a bit

pthread_mutex_t all_finished_mutex;

pthread_cond_t all_finished_cond;

int finished = 0;

int robots_initialize(void)

{

        int i;

        if (signal(SIGCHLD, sig_chld_handler) == SIG_ERR)

                return -1;

        robots_size = 10;

        robots = (Robot*)malloc(sizeof(Robot) * robots_size);

        if (!robots)

                return -1;

        memset(robots, 0, sizeof(Robot) * robots_size);

        for (i = 0; i < robots_size; i++)

                robots[i].id = -1;

        finished = 0;

        pthread_mutex_init(&all_finished_mutex, NULL);

        pthread_cond_init(&all_finished_cond, NULL);

        return 0;

}

/**

 * Creates a new robot. Returns its id on

 * success, or a negative integer on failure.

 **/

int robot_create(char *execname)

{

          int pid,i;

        int id = first_available_id;

        Robot* r = &robots[id];

        // do shared memory stuff here

        key_t key = IPC_PRIVATE;

        //Memory accessible only to children with r/w privileges

        r->sharedMemID = shmget(key, sizeof(RobotShared), IPC_CREAT | 0666);

        //hack!! TODO: remove!!

        r->pose.x=first_available_id*100;

        r->pose.y=first_available_id*50;

        if(r->sharedMemID < 0)

        {

                fprintf(stderr, "Failed to get shared memory.\n");

                return -1;

        }

        r->shared = (RobotShared*)shmat(r->sharedMemID, NULL, 0);

        if(!(r->shared))

        {

                //Free shared memory region

                if (!shmctl(r->sharedMemID, IPC_RMID, NULL))

                        fprintf(stderr, "Failed to free shared memory.\n");

                fprintf(stderr, "Error attaching memory to parent.\n");

                return -1;

        }

        // Initialize robot structure here

        r->shared->motor1 = 0;

        r->shared->motor2 = 0;

        r->id = id;

        if(execname != NULL)

        {

                if((pid = fork()) < 0)

                {

                        //Free Shared Memory Region

                        if (!shmdt(r->shared))

                                fprintf(stderr, "Failed to free shared memory.\n");

                        if (!shmctl(r->sharedMemID, IPC_RMID, NULL))

                                fprintf(stderr, "Failed to free shared memory.\n");

                        r->id = -1;

                        fprintf(stderr, "Failed to fork robot process.\n");

                        return -1;

                }

        }

        if(!pid && execname)

        {

          char var[21];

          /* restore default sigchld handler */

          signal(SIGCHLD, SIG_DFL);

          sprintf(var, "memory_id=%d", r->sharedMemID);

          putenv(var);

          //TODO: keep the other env. stuff around

          execv(execname, NULL);

          err(errno, "exec failed to run child process.\n");

        }

        else

        {

                r->pid = pid;

                do {

                        first_available_id++;

                } while (first_available_id < robots_size &&

                                robots[first_available_id].id != -1);

                // resize the array if necessary

                if (first_available_id >= robots_size)

                {

                        robots_size *= 2;

                        robots = realloc(robots, sizeof(Robot) * robots_size);

                        if (!robots)

                        {

                                fprintf(stderr, "Out of memory.\n");

                                return -1;

                        }

                        for (i = robots_size=first_available_id; i < robots_size; i++)

                          robots[i].id = -1;

                }

        }

        //Log creation

        if(execname != NULL)

        {

                //printf("commiting %d\n", num_robots);

                commit(r, num_robots, CREATE_STEP);

        }

        num_robots++;

        return id;

}

/**

 * Frees all resources associated with a robot, including

 * killing its process if necessary.

 *

 * @return zero on success, nonzero on failure

 **/

int robot_destroy(int id)

{

        Robot* r;

        if (id < 0 || id >= robots_size)

                return -1;

        r = &robots[id];

        if (r->id == 0)

                return -1;

    if (!shmdt(r->shared))

        {

                fprintf(stderr, "Failed to free shared memory.\n");

                return -1;

        }

        if (!shmctl(r->sharedMemID, IPC_RMID, NULL))

        {

                fprintf(stderr, "Failed to free shared memory.\n");

                return -1;

        }

        memset(r, 0, sizeof(Robot));

        r->id = -1;

        if (id < first_available_id)

                first_available_id = 0;

        return 0;

}

void robot_destroy_all()

{

        int i;

        for(i = 0; i < num_robots; i++)

                robot_destroy(i);

}

void robot_iterator_reset(void)

{

        iterator_pos = -1;

}

// note: addresses may change, replace with

// allocated memory

Robot* robot_iterator_next(void)

{

        if (iterator_pos >= robots_size)

                return NULL;

        do iterator_pos++;

        while (iterator_pos < robots_size &&

                        robots[iterator_pos].id == -1);

        if (iterator_pos >= robots_size)

                return NULL;

        return &(robots[iterator_pos]);

}

void sig_chld_handler(int sig)

{

  int ret,stat;

  pthread_mutex_lock(&all_finished_mutex);

  while((ret = waitpid(-1, &stat, WUNTRACED | WNOHANG)))

  {

    if(ret==-1)

      err(errno,"error waiting on robot");

    if(WIFSTOPPED(stat))

      finished++;

  }

  if (finished >= num_robots)

  {

    pthread_cond_signal(&all_finished_cond);

  }

  pthread_mutex_unlock(&all_finished_mutex);

}

void* robot_event_loop(void* arg)

{

        int i;

        int ret;

        //TODO: not exit if there is an error?

        while (1)

        {

                ret = pthread_mutex_lock(&all_finished_mutex);

                if(ret)

                  err(errno, "error locking mutex in even loop");

                // TODO: race condition for adding robots?

                if (finished < num_robots)

                {

                        ret = pthread_cond_wait(&all_finished_cond, &all_finished_mutex);

                        if(ret)

                          err(errno, "error waiting on condition variable");

                }

                finished = 0;

    //sleep to avoid 100% CPU usage

    usleep(10000);

                ret = pthread_mutex_unlock(&all_finished_mutex);

                if(ret)

                  fprintf(stderr, "hmmm, error unlocking. errno: %d", errno);

                timeStep++;

                for (i = 0; i < robots_size; i++)

                        if (robots[i].id != -1)

                        {

                                robot_update(i);

                                printf("commit %d\n", i);

                                commit(&robots[i],i,timeStep);

                        }

                for (i = 0; i < robots_size; i++)

                {

                        if (robots[i].id == -1)

                                continue;

                        ret = kill(robots[i].pid, SIGCONT);

                        if(ret)

                          warn("error: could not kill resume robot proc.");

                }

                gui_refresh();

        }

}

void* logger_step_loop(void* arg)

{

        printf("going into logger\n");

        int timeStep;

        int index;

        int prevStep = -1;

        if(fread(&timeStep, sizeof(int), 1, file) < 1)

        {

                robot_destroy_all();

                fprintf(stderr, "problem reading timestep\n");

            return (void*)-1;

        }

         while(1){

                prevStep = timeStep;

                do

                   {

                        if(fread(&index, sizeof(int), 1, file) < 1)

                        {

                                robot_destroy_all();

                                 return (void*) -1;

                        }

            //Update the robot

                        if(timeStep == CREATE_STEP)

                        {

                            robot_create(NULL);

                        }

                        if(getLog(&robots[index]) < 0)

                        {

                                robot_destroy_all();

                                 return (void*) -1;

                        }

                        //Read in next logfile and check number

                           if(fread(&timeStep, sizeof(int), 1, file) < 1)

                        {

                                robot_destroy_all();

                                return (void*)-1;

                        }

                   if (prevStep < 0)

                                   prevStep = timeStep;

                   }while(timeStep == prevStep || timeStep == CREATE_STEP);

                gui_refresh();

                usleep(100);

        }

}

void robot_update(int i)

{

        Robot* r = &(robots[i]);

        if (r->id == -1)

                return;

        move_robot(r);

        update_rangefinders(r);

}