fsmloc.c

/*
 *  @(#)locfsm.c                07/04/19
 *
 * Finite state machine of the localization part.
 *
 * Copyright    : (c) 2007 DCE Eurobot Dragon Team
 *                CTU FEE - Department of Control Engineering
 * Contacts     : Tran Duy Khanh <trandk1@fel.cvut.cz>
 * License      : GNU GPL v.2
 */

#define FSM_LOC
#include <math.h>
#include <robomath.h>
#include <laser-nav.h>
#include <mcl.h>
#include "robot.h"
#include <fsm.h>
#include "fsmmain.h"

void loc_pln_update();
int loc_mcl_update(struct mcl_model *mcl, double dx, double dy, double dangle);
void dbg_print_parts();

FSM_STATE_DECL(loc_init);
FSM_STATE_DECL(loc_run);
FSM_STATE_DECL(loc_mcl_init);
FSM_STATE_DECL(loc_update);

FSM_STATE(loc_init)
{
        /* set reference points. Used to calculate position of the robot
         * using laser */
        pln_set_points();
        FSM_TIMEOUT(1000);
        FSM_TRANSITION(loc_mcl_init);
}

FSM_STATE(loc_mcl_init)
{
        struct mcl_model *mcl = (struct mcl_model *)&robot.mcl;

        /* playground size */
        mcl->width = PLAYGROUND_WIDTH_MM;
        mcl->height = PLAYGROUND_HEIGHT_MM;

        /* the noises */
        mcl->gen_dnoise = 0.99;         /* distance noise */
        mcl->gen_anoise = DEGREES(10);  /* angle noise */
        mcl->mov_dnoise = 1.1;
        mcl->mov_anoise = 2.0;
        mcl->w_min = 0.25;
        mcl->w_max = 2.0;
        mcl->eval_sigma = 0.5;
        mcl->maxavdist = 50;
        mcl->maxnoisecycle = 10;        /* bad cycles before reseting */

        /* amount of particles */
        mcl->count = 5000;
        mcl->parts = (struct mcl_particle *)
                        malloc(sizeof(struct mcl_particle)*mcl->count);

        /* phases of the MCL */
        mcl->init = mcl_init;
        mcl->move = mcl_move;
        mcl->update = mcl_update;
        mcl->normalize = mcl_normalize;
        mcl->sort = mcl_partsort;
        mcl->resample = mcl_resample;

        /* cycles of enumeration */
        mcl->cycle = 0;
        mcl->noisecycle = 0;

        /* generate particles with noises */
        mcl->init(mcl);
        printf("loc_mcl_init: done\n");

        /* change flag */
        mcl->flag = MCL_RUN;

        /* print generated particles */
        /*dbg_print_parts();*/

        FSM_TRANSITION(loc_run);
}

FSM_STATE(loc_run)
{
        DBG_PRINT_EVENT("loc_run event:");

        FSM_TIMEOUT(1000);
        switch (FSM_EVENT) {
                case EV_LOC_INIT:
                        /*DBG_PRINT_EVENT("mcl_init event:");*/
                        FSM_TRANSITION(loc_mcl_init);
                        break;
                case EV_TIMEOUT:
//                      FIXME: dont forget to uncomment the break expression bellow
//                      when the laser works.
//                      DBG_PRINT_EVENT("timeout event:");
//                      break;
                case EV_LOC_LOCATE:
                case EV_LASER_RECEIVED:
                        SUBFSM_TRANSITION(loc_update);
                        break;
                default:
                        /*DBG_PRINT_EVENT("loc_run:unhandled event:");*/
                        ;
        }
}

FSM_STATE(loc_update)
{
        int dx, dy, dangle;
        int rv;

        DBG_PRINT_EVENT("loc_update:");

        /* FIXME: here should be data from the odometry */
        ROBOT_LOCK(act_pos);
        dx = 1.0;
        dy = 1.0;
        dangle = 0.0;
        ROBOT_UNLOCK(act_pos);

        /* calculate measured position */
        loc_pln_update();

        /* update mcl with measured position */
        rv = loc_mcl_update(&robot.mcl, (double)dx, (double)dy, (double)dangle);

        /* send update signal if mcl update has finished successful */
        if (!rv)
                FSM_SIGNAL(MOTION, EV_LOC_UPDATED);
        else {
                printf("MCL_RESET: LOST!\n");
                FSM_SIGNAL(MOTION, EV_LOC_LOST);
        }

        SUBFSM_RET();
}

void loc_pln_update()
{
        struct pln_pos_state _act_pos;
        struct pln_pos_state _cal_pos;
        unsigned int times[LAS_CNT], cnt;
        int i;

        /* measured times from laser beacon */
        ROBOT_LOCK(laser_recv);
        /* FIXME: just a hack, until the laser will 
         * work correctly */
        robot.laser_recv.cnt = 5;
        robot.laser_recv.period = 54619;
        robot.laser_recv.measures[0] = 1714;
        robot.laser_recv.measures[1] = 12312;
        robot.laser_recv.measures[2] = 34549;
        robot.laser_recv.measures[3] = 41231;
        robot.laser_recv.measures[4] = 49670;

        cnt = robot.laser_recv.cnt + 1;
        times[0] = robot.laser_recv.period;
        for (i=0; i<robot.laser_recv.cnt; i++)
                times[i+1] = robot.laser_recv.measures[i];
        ROBOT_UNLOCK(laser_recv);

//      printf("cnt=%d period=%d measures: ", 
//              robot.laser_recv.cnt, robot.laser_recv.period);
//      for (i=0; i<robot.laser_recv.cnt; i++) {
//              printf("%d ", robot.laser_recv.measures[i]);
//      }
//      printf("\n");
        printf("times:");
        for (i=0; i<cnt; i++)
                printf("%d ", times[i]);
        printf("\n");

        /* actual position */
        /*ROBOT_LOCK(act_pos);
        _act_pos.x = robot.act_pos.x;
        _act_pos.y = robot.act_pos.y;
        _act_pos.head = robot.act_pos.phi;
        ROBOT_UNLOCK(act_pos);*/
        /* FIXME: this stays here just until the odometry will be done */
        _act_pos.x = 500;
        _act_pos.y = 1600;
        _act_pos.head = 0;
        printf("act: x=%f y=%f h=%f\n", _act_pos.x, _act_pos.y, _act_pos.head);

        /* calculated measured position */
        pln_cal_position(times, cnt, _act_pos, &_cal_pos);
        printf("calculated posititon: [x=%f,y=%f]\n", _cal_pos.x, _cal_pos.y);

        /* set currently calculated position */
        ROBOT_LOCK(est_pos);
        robot.est_pos.x = _cal_pos.x;
        robot.est_pos.y = _cal_pos.y;
        robot.est_pos.phi = _cal_pos.head;
        ROBOT_UNLOCK(est_pos);
}

int loc_mcl_update(struct mcl_model *mcl, double dx, double dy, double dangle)
{
        struct mcl_particle est_pos;
        struct mcl_particle cal_pos;

        /* get calculated position by laser */
        ROBOT_LOCK(est_pos);
        cal_pos.x = robot.est_pos.x;
        cal_pos.y = robot.est_pos.y;
        cal_pos.angle = robot.est_pos.phi;
        ROBOT_UNLOCK(est_pos);

        mcl->cycle++;
        printf("loc_mcl_update: cycle = %d\n", mcl->cycle);

        /* move particles */
        mcl->move(mcl, dx, dy, dangle);

        /* update particles with calculated position from measured times */
        mcl->update(mcl, cal_pos.x, cal_pos.y, cal_pos.angle);

        /* we got lost, reset mcl */
        if (mcl->flag == MCL_RESET)
                return 1;

        mcl->normalize(mcl);
        mcl->sort(mcl);
        mcl->resample(mcl);
        est_pos = mcl_get_pos(mcl);

        ROBOT_LOCK(est_pos);
        robot.est_pos.x = est_pos.x;
        robot.est_pos.y = est_pos.y;
        robot.est_pos.phi = est_pos.angle;
        ROBOT_UNLOCK(est_pos);

        printf("estimated position: %f %f %f %f\n", 
                est_pos.x, est_pos.y, est_pos.angle, est_pos.weight);

        return 0;
}

/* Debug prints */
void dbg_print_parts()
{
        int i;

        for (i=0; i<robot.mcl.count; i++)
                printf("%d) x=%f y=%f a=%f w=%f\n", 
                        i,
                        ((struct mcl_particle *)(robot.mcl.parts))[i].x,
                        ((struct mcl_particle *)(robot.mcl.parts))[i].y,
                        ((struct mcl_particle *)(robot.mcl.parts))[i].angle,
                        ((struct mcl_particle *)(robot.mcl.parts))[i].weight);
        printf("robot: x=%f y=%f angle=%f\n",
                ((struct mcl_particle *)(robot.mcl.system))->x,
                ((struct mcl_particle *)(robot.mcl.system))->y,
                ((struct mcl_particle *)(robot.mcl.system))->angle);

}

---