src/robofsm/fsmloc.c

   1 /*
   2  * fsmloc.c                     08/04/20
   3  *
   4  * Finite state machine of the localization part.
   5  *
   6  * Copyright    : (c) 2008 CTU Dragons
   7  *                CTU FEE - Department of Control Engineering
   8  * License      : GNU GPL v.2
   9  */
  10
  11 #define FSM_LOC
  12 #include <math.h>
  13 #include <robomath.h>
  14 #include <laser-nav.h>
  15 #include <mcl.h>
  16 #include <robodata.h>
  17 #include <robot.h>
  18 #include <fsm.h>
  19
  20 struct mcl_laser l;
  21 struct mcl_model *mcl;
  22 //struct mcl_laser_measurement meas_angles;
  23
  24 FSM_STATE_DECL(init);
  25 FSM_STATE_DECL(run);
  26 FSM_STATE_DECL(ignore_laser);
  27
  28 /**
  29  * FSM state: first state. Initilization.
  30  */
  31 FSM_STATE(init)
  32 {
  33         FSM_TRANSITION(run);
  34 }
  35
  36 /**
  37  * FSM state: main state. Every state will return back to this one.
  38  */
  39 FSM_STATE(run)
  40 {
  41         struct mcl_model *mcl = robot.mcl;
  42         switch (FSM_EVENT) {
  43                 case EV_RESET:
  44                         printf("MCL reset\n");
  45                         mcl->reset(mcl);
  46                         FSM_TIMER(3000);
  47                         break;
  48                 case EV_TIMER:
  49                         FSM_SIGNAL(MOTION, EV_FOUND_AFTER_RESET, NULL);
  50                         break;
  51                 case EV_ODO_RECEIVED: {
  52                         struct mcl_robot_odo *odo = FSM_EVENT_PTR;
  53                         struct mcl_robot_pos *est = robot.mcl->estimated;
  54                         mcl->predict(mcl, odo);
  55                         free(odo);
  56                         ROBOT_LOCK(est_pos);
  57                         robot.est_pos.x = est->x;
  58                         robot.est_pos.y = est->y;
  59                         robot.est_pos.phi = est->angle;
  60                         ROBOT_UNLOCK(est_pos);
  61                         break;
  62                 }
  63                 case EV_LASER_RECEIVED: {
  64                         struct mcl_laser_measurement *meas = FSM_EVENT_PTR;
  65                         if (robot.laser_enabled) {
  66                                 struct mcl_robot_pos *est = robot.mcl->estimated;
  67                                 mcl->update(mcl, meas);
  68
  69                                 ROBOT_LOCK(est_pos);
  70                                 robot.est_pos.x = est->x;
  71                                 robot.est_pos.y = est->y;
  72                                 robot.est_pos.phi = est->angle;
  73                                 ROBOT_UNLOCK(est_pos);
  74                         }
  75
  76                         free(meas);
  77                         /* DBG("est_pos=[%f,%f,%f]\n", robot.est_pos.x, robot.est_pos.y, robot.est_pos.phi); */
  78
  79                         robot.mcl->resample(robot.mcl);
  80                         break;
  81                 }
  82                 case EV_SET_ESTIMATED: {
  83                         struct mcl_robot_pos *pos = FSM_EVENT_PTR;
  84                         mcl->set_estimated(mcl, pos);
  85                         free(pos);
  86                         break;
  87                 }
  88                 case EV_ENTRY:
  89                 case EV_EXIT:
  90                         break;
  91                 case EV_RETURN:
  92                         DBG_PRINT_EVENT("unhandled event");
  93                         break;
  94         }
  95 }