robot.c

#include "robot.h"
#include <signal.h>
#include "fsmmove.h"
#include <unistd.h>
#include "serva.h"
#include "movehelper.h"

static int thread_priorities[] = {
        [FSM_ID_MOTION] = 25,
        [FSM_ID_MAIN] = 20,
        [FSM_ID_DET] = 15,
        [FSM_ID_LOC] = 10
};

static char *fsm_name[] = {
        [FSM_ID_MOTION] = "motion",
        [FSM_ID_MAIN] = "main",
        [FSM_ID_DET] = "detection",
        [FSM_ID_LOC] = "localization",
        [FSM_ID_PICKUP] = "pickup"
};

struct robo_priv robot;

static void block_signals()
{
        sigset_t sigset;
        int i;

        sigemptyset(&sigset);
        for (i=0; i<7; i++)
                sigaddset(&sigset, SIGRTMIN+i);

        pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*)NULL);
}

static void int_handler(int sig)
{
        robot_exit();
}

int fsm_det_in(struct robo_fsm *fsm, int ret_code);

int robot_init()
{
        int i;
        robot.mode = ROBO_TESTING;

        pthread_mutex_init(&robot.lock, NULL);
        pthread_mutex_init(&robot.lock_act_pos, NULL);

        /* FSM initialization */
        for (i=0; i<FSM_CNT; i++)
                fsm_init(&robot.fsm[i], fsm_name[i]);
        
        signal(SIGINT, int_handler);
        block_signals();

        robot.serva.release = FRONT_LAUNCH_LOCK;
        robot.serva.frontDoor = FRONT_DOOR_UP;
        robot.serva.innerDoor = INNER_DOOR_UP;
        robot.serva.backDoor = BACK_DOOR_UP;
        robot.serva.transporterFront = FRONT_TRANSPORTER_STOP;
        robot.serva.transporterInner = INNER_TRANSPORTER_STOP;

        robot.sharpsOpponent.longSharpDist1 = 2.0;
        robot.sharpsOpponent.longSharpDist2 = 2.0;
        robot.sharpsOpponent.longSharpDist3 = 2.0;

        initOrte();
        ORTEPublicationSend(robot.publisherServa);
        
        return 0;
}
int robot_start()
{
        int i;

        /* start FSM threads */
        for(i=0; i<FSM_CNT; i++) {
                pthread_attr_t attr;
                struct sched_param param;

                pthread_attr_init(&attr);

                param.sched_priority = thread_priorities[i];
                pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
                if (pthread_attr_setschedparam(&attr, &param)) {
                        perror("pthread_attr_setschedparam");
                        exit(1);
                }

                
                if (fsm_start(&robot.fsm[i], &attr) != 0) {
                        perror("fsm_start");
                        exit(1);
                }
        }

        return 0;
}

int robot_exit()
{
        int i;
        /* start FSM threads */
        for(i=0; i<FSM_CNT; i++) {
                //robot.fsm[i].exit_flag=1;
                __fsm_signal(&robot.fsm[i], EV_EXIT);
        }
        return 0;
}

int robot_wait()
{
        int i;
        /* wait for threads */
        for (i=0; i<FSM_CNT; i++) {
                pthread_join(robot.fsm[i].threadid, NULL);
        }
        return 0;
}

int robot_destroy()
{
        int i;
        printf("\n\n\n");
        for (i=0;i<3;i++) {     /* For better reliability :( */
                robot_trajectory_stop();
                frontDoorDown();
                frontDoorUp();
                innerDoorUp();
                backDoorUp();
                frontTransporterStop();
                innerTransporterStop();
                lockFront();
                usleep(100000);
        }

        for (i=0; i<FSM_CNT; i++)
                fsm_destroy(&robot.fsm[i]);
        DBG("robofsm: stop.\n");
        terminateOrte();
        return 0;
}

---