fsmmove.cc

Go to the documentation of this file.

#define FSM_MOTION
#include <sys/time.h>
#include <time.h>
#include "fsmmove.h"
#include "trgen.h"
#include "balet.h"
#include "robot.h"
#include <pthread.h>
#include <path_planner.h>
#include <signal.h>
#include "movehelper.h"
#include <sharp.h>
#include <unistd.h>
#include <map.h>
#include "robot_config.h"


/*******************************************************************************
 * Parameters of Obstacle detection
 *******************************************************************************/

#define OBS_PERIOD_WATCH        100000          
#define OBS_PERIOD_BETWEEN_DET  1000000         
#define OBS_CSPACE              (200/MAP_CELL_SIZE)     
#define OBS_FORGET_PERIOD       100000          
#define OBS_UPDATE_MAP_SPEED 15                 
/*******************************************************************************
 * Controller thread and helper functions for that thread
 *******************************************************************************/


//Controller gains
const double k[3] =
        {
                2,               // dx gain
                2,               // dphi gain
                10
        }
        ;             // dy gain
#define MAX_POS_ERROR_M 0.5
#define CLOSE_TO_TARGET_M 0.1
#define MAX_WAIT_FOR_CLOSE_MS 2000

//#define MOTION_PERIOD_NS (500/*ms*/*1000*1000)
#define MOTION_PERIOD_NS (50/*ms*/*1000*1000)
#define SIG_TIMER (SIGRTMIN+0)
#define SIG_NEWDATA (SIGRTMIN+1)

// Global varibles
static pthread_t thr_trajctory_follower;
static pthread_t thr_trajctory_recalc;
static pthread_t thr_update_map;
static struct timeval tv_start; 
static Trajectory *actual_trajectory = NULL;

//static Trajectory *switch_to_trajectory = NULL;
//static double switch_time;

// function definition
static fsm_event new_goal(void);
static void stop();

bool actual_trajectory_reverse;
pthread_mutex_t actual_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t switch_to_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;

// static void odometry(double l, r) {
//      static double last_l, last_r;

//      robot.act_pos.x = (double)ROBOT_AXIS_TO_BACK_MM/1000;
//      robot.act_pos.y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2)/1000;
//      robot.act_pos.phi = 0;

//      ROBOT_LOCK(act_pos);
//      ROBOT_UNLOCK(act_pos);
// }

void delete_actual_trajectory()
{
        Trajectory *old;
        pthread_mutex_lock(&actual_trajectory_lock);
        old = actual_trajectory;
        actual_trajectory = NULL;
        pthread_mutex_unlock(&actual_trajectory_lock);
        robot_send_speed(0,0);
        if (old) delete(actual_trajectory);
}

static void notify_fsm(bool done, double error)
{
        static bool done_sent;

        if (error > MAX_POS_ERROR_M) {
                FSM_SIGNAL(MOTION, EV_TRAJECTORY_LOST);
        } else {
                if (done) {
                        if (error < CLOSE_TO_TARGET_M) {
                                FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE_AND_CLOSE);
                        } else if (!done_sent) {
                                done_sent = true;
                                FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE);
                        }
                } else {
                        done_sent = false;
                }
        }
}

static void *trajectory_follower(void *arg)
{
        struct itimerspec ts;
        sigset_t sigset;
        struct sigevent sigevent;
        timer_t timer;

        ts.it_value.tv_sec = 0;
        ts.it_value.tv_nsec = MOTION_PERIOD_NS;
        ts.it_interval.tv_sec = 0;
        ts.it_interval.tv_nsec = MOTION_PERIOD_NS;

        sigemptyset(&sigset);
        sigaddset(&sigset, SIG_TIMER);
        sigaddset(&sigset, SIG_NEWDATA);

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

        sigevent.sigev_notify = SIGEV_SIGNAL;
        sigevent.sigev_signo = SIG_TIMER;
        sigevent.sigev_value.sival_int = 0;

        if (timer_create(CLOCK_REALTIME, &sigevent, &timer) < 0) {
                perror("move: timer_create");
                return NULL;
        }

        if (timer_settime(timer, 0, &ts, NULL) < 0) {
                perror("move: timer_settimer");
                return NULL;
        }

        while (1) {
                double speeds[2];
                int sig;
                // Wait for start of new period or new data
                sigwait(&sigset, &sig);
                //DBG("signal %d ", sig - SIGRTMIN);
                pthread_mutex_lock(&actual_trajectory_lock);
                Trajectory *w = actual_trajectory;
                if (w) {
                        Pos ref_pos;
                        bool done;
                        struct position_type ap;
                        double t;
                        struct timeval tv;

                        // Calculate reference position
                        gettimeofday(&tv, NULL);
                        t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
                        t += (tv.tv_sec - tv_start.tv_sec);


                        done = w->getRefPos(t, ref_pos);
                        pthread_mutex_unlock(&actual_trajectory_lock);

                        ROBOT_LOCK(act_pos);
#ifdef CONFIG_OPEN_LOOP
                        // We don't use any feedback now. It is
                        // supposed that the actual position is equal
                        // to reference position.
                        robot.act_pos.x = ref_pos.x;
                        robot.act_pos.y = ref_pos.y;
                        robot.act_pos.phi = ref_pos.phi;
#endif
                        ap = robot.act_pos;

                        ROBOT_UNLOCK(act_pos);
                        double aktPos[] = { ap.x, ap.y, ap.phi };
                        DBG("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f\r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v);
                        fflush(stdout);
                        // Call the controller
                        double error;
                        error = balet(ref_pos, aktPos, speeds, k);

                        notify_fsm(done, error);
                } else {
                        pthread_mutex_unlock(&actual_trajectory_lock);
                        speeds[0] = 0;
                        speeds[1] = 0;
                }


                // Apply controller output
                robot_send_speed(speeds[0], speeds[1]);
        }
}

static void * trajctory_recalc(void * arg)
{
        int i,j, xcell, ycell;
        double val, x, y;
        Pos p;
        
        while (1) {
                //ShmapUpdateTmpObstacles(OBS_UPDATE_MAP_SPEED);
                ROBOT_LOCK(sharpsOpponent);
                val = (robot.sharpsOpponent.longSharpDist1 + robot.sharpsOpponent.longSharpDist2 +
                       robot.sharpsOpponent.longSharpDist3)/3.0;
                ROBOT_UNLOCK(sharpsOpponent);
                
                if ((val<0.8)){
                        int flag_recalc;        // This flag indicates if the path should be recalculated
                        int flag_goal_obst;     // This flag indicates that the goal is an obstacle

                        flag_recalc = 0;
                        flag_goal_obst = 0;
                        
                        ROBOT_LOCK(act_pos);
                        p.x = robot.act_pos.x;
                        p.y = robot.act_pos.y;
                        p.phi = robot.act_pos.phi;
                        ROBOT_UNLOCK(act_pos);
                        x = val*cos(p.phi) + p.x;
                        y = val*sin(p.phi) + p.y;
                        DBG("New obstacle detected at cell [%f,%f] \n", x,y);
                        if (ShmapIsFreePoint(x,y)){
                                xcell=ShmapPoint2Cell_X(x);ycell=ShmapPoint2Cell_Y(y);
                                DBG("Cell %d, %d\n", xcell, ycell);
                                for (i=(xcell-OBS_CSPACE); i <= xcell+OBS_CSPACE; i++) {
                                        for (j=(ycell- OBS_CSPACE); j <=ycell + OBS_CSPACE; j++) {
                                                switch (ShmapGetCellValue(i,j)){
                                                        case MAP_PATH:
                                                        case MAP_GOAL:
                                                                flag_recalc =1;
                                                        case MAP_FREE:
                                                                ShmapSetCellValue(i , j, MAP_NEW_OBSTACLE);
                                                        break;
                                                        case MAP_WALL:
                                                        case MAP_WALL_CSPACE:
                                                        case MAP_NEW_OBSTACLE:
                                                        case MAP_NEW_OBSTACLE_CSPACE:
                                                        case MAP_NOT_IN_MAP:
                                                        default:
                                                                break;
                                                }
                                        }
                                }

                                // Path recalculation if the goal is not an obstacle and the path should be recalc
                                if (flag_goal_obst) {
                                        FSM_SIGNAL(MOTION, EV_MOVE_STOP);
                                        FSM_SIGNAL(MAIN, EV_GOAL_IS_TMP_OBSTACLE);
                                } else if (flag_recalc){
                                        if (new_goal() == 0) DBG("Path recalc\n");
                                } else {
                                        DBG("Not necessary to recalc the path\n");
                                }
                        } else DBG("The detected obstacle is already in the map\n");
                        
                        //waits between detection
                        usleep(OBS_PERIOD_BETWEEN_DET);
                }
                usleep(OBS_PERIOD_WATCH);
        }
}

static void * thread_update_map(void * arg){
        while (1) {
                ShmapUpdateTmpObstacles(OBS_UPDATE_MAP_SPEED);
                usleep(OBS_FORGET_PERIOD);
        }
}

static void go(Trajectory *t)
{
        Trajectory *old;
        pthread_mutex_lock(&actual_trajectory_lock);
        old = actual_trajectory;
        gettimeofday(&tv_start, NULL);
        actual_trajectory = t;
        pthread_mutex_unlock(&actual_trajectory_lock);
        if (old)
                delete(old);
}

static void stop()
{
        delete_actual_trajectory();
        pthread_kill(thr_trajctory_follower, SIG_NEWDATA);
}


static int new_goal(void)
{
        double startx, starty, goalx, goaly, angle;
        PathPoint * path = NULL;
        PathPoint * tmp_point = NULL;
        
        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
        Pos start;
        //tc.maxv *= 2.5;
        //tc.maxacc *= 2.5;

        ROBOT_LOCK(act_pos);
        startx = robot.act_pos.x;
        starty = robot.act_pos.y;
        start.x = startx;
        start.y = starty;
        start.phi = robot.act_pos.phi;
        ROBOT_UNLOCK(act_pos);
        ROBOT_LOCK(goal);
        goalx = robot.goal.x;
        goaly = robot.goal.y;
        ROBOT_UNLOCK(goal);

        //DBG("Going from (%.2f, %.2f) to (%.2f, %.2f)\n", startx, starty, goalx, goaly);
        switch (path_planner(startx, starty, goalx, goaly, &path, &angle)) {
                case 1:
                        break;
                case PP_ERROR_MAP_NOT_INIT:
                        FSM_SIGNAL(MAIN, EV_MAP_NOT_INIT);
                        return -1;
                        break;
                case PP_ERROR_GOAL_IS_OBSTACLE:
                        FSM_SIGNAL(MAIN, EV_GOAL_IS_OBSTACLE);
                        return -1;
                        break;
                case PP_ERROR_GOAL_NOT_REACHABLE:
                        FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE);
                        return -1;
                        break;
                default:
                        break;
                // shouldn't we call freePathMemory() here? No, because no memory has been allocanted for the path, path is only a pointer to NULL
        }

        bool backward = false;
        Trajectory *t = new Trajectory(tc, backward);

        // Add this  path to the trajectory.
        for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) {
                //DBG("ADDDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y);
                t->addPoint(tmp_point->x, tmp_point->y);
        }

        // add the path to the robot trajectory
        t->finalHeading = angle; // NAN should be OK here, if not, it is error.

        // Free path allocated memory
        freePathMemory(path);

        if (t->prepare(start)) {
                go(t);
        } else {
                DBG("ERROR preparing trajectory\n");
                delete(t);
                FSM_SIGNAL(MAIN, EV_TRAJECTORY_ERROR);
                return -1;
        }

        return 0;
}


static int new_trajectory(void)
{
        Trajectory *t;
        Pos pos;
        ROBOT_LOCK(new_trajectory);
        t = (Trajectory*) robot.new_trajectory;
        if (t) robot.new_trajectory = NULL;
        else {
                ROBOT_UNLOCK(new_trajectory);
                DBG("No trajectory\n");
                FSM_SIGNAL(MAIN, EV_TRAJECTORY_ERROR);
                return -1;
        }
        ROBOT_UNLOCK(new_trajectory);


        ROBOT_LOCK(act_pos);
        pos.x = robot.act_pos.x;
        pos.y = robot.act_pos.y;
        pos.phi = robot.act_pos.phi;
        ROBOT_UNLOCK(act_pos);

        if (t->prepare(pos)) {
                go(t);
        } else {
                FSM_SIGNAL(MAIN, EV_MOTION_DONE); // FIXME: Is this correct?
                return -1;
        }
        return 0;
}

/*******************************************************************************
 * The automaton
 *******************************************************************************/

FSM_STATE_DECL(movement);
FSM_STATE_DECL(close_to_target);
FSM_STATE_DECL(wait_for_command);


FSM_STATE(move_init)
{
        pthread_attr_t tattr;
        sched_param param;
        int ret;

        if (FSM_EVENT == EV_INIT) {
                actual_trajectory = NULL;

                // Trajectory follower thread
                pthread_attr_init (&tattr);
                pthread_attr_getschedparam (&tattr, &param);
                param.sched_priority = TRAJ_FOLLOWER_PRIO;
                ret = pthread_attr_setschedparam (&tattr, &param);
                if(ret) {
                        perror("pthread_attr_setschedparam");
                        exit(1);
                }
                ret = pthread_create(&thr_trajctory_follower, &tattr, trajectory_follower, NULL);
                if(ret) {
                        perror("pthread_create");
                        exit(1);
                }
                // Thread of trajectory recalc
                pthread_attr_init (&tattr);
                pthread_attr_getschedparam (&tattr, &param);
                param.sched_priority = TRAJ_RECALC_PRIO;
                ret = pthread_attr_setschedparam (&tattr, &param);
                if(ret) {
                        perror("pthread_attr_setschedparam");
                        exit(1);
                }
                pthread_create(&thr_trajctory_recalc, NULL, trajctory_recalc, NULL);
                
                // Thread update Map
                pthread_attr_init (&tattr);
                pthread_attr_getschedparam (&tattr, &param);
                param.sched_priority = TRAJ_RECALC_PRIO;
                ret = pthread_attr_setschedparam (&tattr, &param);
                if(ret) {
                        perror("pthread_attr_setschedparam");
                        exit(1);
                }
                pthread_create(&thr_update_map, NULL, thread_update_map, NULL);
                
                FSM_TRANSITION(wait_for_command);
        }
}

FSM_STATE(wait_for_command)
{
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        stop();
                        break;
                case EV_NEW_GOAL:
                        if (new_goal() != 0) {
                                // The event must be the return value of new_goal()
                                DBG("Goal error\n");
                                break;
                        }
                        FSM_TRANSITION(movement);
                        break;
                case EV_NEW_TRAJECTORY:
                        if (new_trajectory() != 0) {
                                break;
                        }
                        FSM_TRANSITION(movement);
                        break;
                default:
                        break;
        }
}



FSM_STATE(movement)
{
        switch (FSM_EVENT) {
                case EV_TRAJECTORY_DONE:
                        FSM_TRANSITION(close_to_target);
                        break;
                case EV_TRAJECTORY_DONE_AND_CLOSE:
                        FSM_SIGNAL(MAIN, EV_MOTION_DONE);
                        FSM_TRANSITION(wait_for_command);
                        break;
                case EV_TRAJECTORY_LOST:
                        FSM_SIGNAL(MAIN, EV_LOST_DURING_MOTION);
                        FSM_TRANSITION(wait_for_command);
                        break;
                case EV_MOVE_STOP:
                        FSM_TRANSITION(wait_for_command);
                        break;
                default:
                        // TODO: Macro FSM_DEFAULT prints an ungandled event
                        break;
        }
}


FSM_STATE(close_to_target)
{
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
                        break;
                case EV_TRAJECTORY_DONE_AND_CLOSE:
                        FSM_SIGNAL(MAIN, EV_MOTION_DONE);
                        FSM_TRANSITION(wait_for_command);
                        break;
                case EV_TRAJECTORY_LOST:
                case EV_TIMER:
                        stop();
                        FSM_SIGNAL(MAIN, EV_LOST_DURING_MOTION);
                        FSM_TRANSITION(wait_for_command);
                        break;
                case EV_MOVE_STOP:
                        FSM_TRANSITION(wait_for_command);
                        break;
                case EV_TRAJECTORY_DONE:
                case EV_NEW_TRAJECTORY:
                case EV_LOC_UPDATED:
                case EV_NEW_GOAL:
                case EV_LOC_LOST:
                case EV_INIT:
                case EV_EXIT:
                case EV_RETURN:
                case EV_TIMEOUT:
                        DBG_PRINT_EVENT("unhandled event");
                        break;
        }
}

---