src/robofsm/robot.cc

   1 #include <map.h>
   2 #include <movehelper.h>
   3 #include <pthread.h>
   4 #include <robomath.h>
   5 #include "robot.h"
   6 #include "guard.hpp"
   7 #include "actuators.h"
   8 #include <robot_orte.h>
   9 #include <signal.h>
  10 #include <sys/time.h>
  11 #include <time.h>
  12 #include <unistd.h>
  13 #include "map_handling.h"
  14 #include <string.h>
  15 #include <robodim.h>
  16 #include <ul_log.h>
  17 #include "fsmmove.cc"
  18
  19
  20 /* Subtract the `struct timespec' values X and Y,
  21    storing the result in RESULT (result = x - y).
  22    Return 1 if the difference is negative, otherwise 0.  */
  23
  24 int timespec_subtract (timespec &result, timespec &x, timespec &y) {
  25   /* Perform the carry for the later subtraction by updating Y. */
  26   if (x.tv_nsec < y.tv_nsec) {
  27     int num_sec = (y.tv_nsec - x.tv_nsec) / 1000000000 + 1;
  28     y.tv_nsec -= 1000000000 * num_sec;
  29     y.tv_sec += num_sec;
  30   }
  31   if (x.tv_nsec - y.tv_nsec > 1000000000) {
  32     int num_sec = (x.tv_nsec - y.tv_nsec) / 1000000000;
  33     y.tv_nsec += 1000000000 * num_sec;
  34     y.tv_sec -= num_sec;
  35   }
  36
  37   /* Compute the time remaining to wait.
  38      `tv_nsec' is certainly positive. */
  39   result.tv_sec = x.tv_sec - y.tv_sec;
  40   result.tv_nsec = x.tv_nsec - y.tv_nsec;
  41
  42   /* Return 1 if result is negative. */
  43   return x.tv_sec < y.tv_sec;
  44 }
  45
  46
  47 Robot robot;
  48
  49 void Robot::int_handler(int sig) {
  50         robot.robot_exit();
  51 }
  52
  53 void Robot::block_signals() {
  54         sigset_t sigset;
  55         int i;
  56
  57         sigemptyset(&sigset);
  58         for (i=SIGRTMIN; i<=SIGRTMAX; i++)
  59                 sigaddset(&sigset, i);
  60
  61         pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*)NULL);
  62 }
  63
  64 Robot::Robot() {
  65         act.set_orte(&orte);
  66 }
  67
  68 Robot::~Robot() {
  69 }
  70
  71
  72 /**
  73  * Initializes the robot.
  74  * Setup fields in robot structure, initializes FSMs and ORTE.
  75  *
  76  * @return 0
  77  */
  78 int Robot::init() {
  79         int rv;
  80         pthread_mutexattr_t mattr;
  81         rv = pthread_mutexattr_init(&mattr);
  82 #ifdef HAVE_PRIO_INHERIT
  83         rv = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
  84 #endif
  85         pthread_mutex_init(&robot.lock, &mattr);
  86         pthread_mutex_init(&robot.lock_ref_pos, &mattr);
  87         pthread_mutex_init(&robot.lock_est_pos_odo, &mattr);
  88         pthread_mutex_init(&robot.lock_est_pos_indep_odo, &mattr);
  89         pthread_mutex_init(&robot.lock_meas_angles, &mattr);
  90         pthread_mutex_init(&robot.lock_joy_data, &mattr);
  91         pthread_mutex_init(&robot.lock_disp, &mattr);
  92         MOTION = sched.create_processor<FSMMotion>();
  93
  94         team_color = VIOLET;
  95
  96         if (team_color == VIOLET) {
  97                 ul_loginf("We are VIOLET!\n");
  98         } else {
  99                 ul_loginf("We are RED!\n");
 100         }
 101
 102         set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG));
 103
 104         ignore_hokuyo = false;
 105         map = ShmapInit(1);
 106 //      fill_in_known_areas_in_map();
 107
 108         signal(SIGINT, int_handler);
 109         signal(SIGTERM, int_handler);
 110         block_signals();
 111
 112         /* initial values */
 113         orte.motion_speed.left = 0;
 114         orte.motion_speed.right = 0;
 115
 116         orte.pwr_ctrl.voltage33 = 1;
 117         orte.pwr_ctrl.voltage50 = 1;
 118         orte.pwr_ctrl.voltage80 = 1;
 119
 120         orte.camera_control.on = true;
 121
 122
 123         /* Only activate display if it is configured */
 124         /* FIXME: display
 125         robot.sercom = uoled_init(serial_comm);
 126         if (strcmp(robot.sercom->devname, "/dev/null") != 0)
 127                 robot.fsm.display.state = &fsm_state_disp_init;
 128         */
 129
 130         obstacle_avoidance_enabled = true;
 131         use_back_bumpers = true;
 132         use_left_bumper = true;
 133         use_right_bumper = true;
 134         start_state = POWER_ON;
 135         check_turn_safety = true;
 136
 137         /* init ORTE domain, create publishers, subscribers, .. */
 138         rv = robot_init_orte();
 139
 140         return rv;
 141 }
 142
 143 /**
 144  * Starts the robot FSMs and threads.
 145  *
 146  * @return 0
 147  */
 148 int Robot::start() {
 149         int rv = 0;
 150
 151         pthread_attr_t tattr;
 152         struct sched_param param;
 153         pthread_t thr_obstacle_forgeting;
 154         int ret;
 155
 156         ret = motion_control_init();
 157         if(ret) {
 158                 perror("motion_control_init");
 159                 robot_exit();
 160         }
 161
 162
 163         /* Obstacle forgeting thread */
 164         pthread_attr_init (&tattr);
 165         pthread_attr_getschedparam (&tattr, &param);
 166         pthread_attr_getschedparam (&tattr, &param);
 167         pthread_attr_setschedpolicy(&tattr, SCHED_FIFO);
 168         param.sched_priority = OBST_FORGETING_PRIO;
 169         rv = pthread_attr_setschedparam (&tattr, &param);
 170         if (rv) {
 171                 perror("robot_start: pthread_attr_setschedparam()");
 172                 goto err;
 173         }
 174         rv = pthread_create(&thr_obstacle_forgeting,
 175                             &tattr, thread_obstacle_forgeting, NULL);
 176         if (rv) {
 177                 perror("robot_start: pthread_create");
 178                 goto err;
 179         }
 180
 181         sched(); // start main loop
 182
 183 err:
 184         return rv;
 185 }
 186
 187 /**
 188  * Signals all the robot threads to finish.
 189  */
 190 void Robot::robot_exit() {
 191         sched.terminate();
 192 }
 193
 194
 195 /**
 196  * Stops the robot. All resources alocated by robot_init() or
 197  * robot_start() are dealocated here.
 198  */
 199 void Robot::destroy() {
 200         motion_control_done();
 201
 202         robottype_roboorte_destroy(&orte);
 203
 204 //      sched.terminate();
 205 //      fsm_destroy(&robot.fsm.main);
 206 //      fsm_destroy(&robot.fsm.motion);
 207 //      fsm_destroy(&robot.fsm.act);
 208         /*robot.map.ShmapFree();*/
 209 //      ul_logdeb("robofsm: stop.\n");
 210 }
 211
 212
 213 void Robot::get_est_pos_trans(double &x, double &y, double &phi) {
 214         get_est_pos(x, y, phi);
 215         x = trans_x(x);
 216         y = trans_y(y);
 217         phi = trans_angle(phi);
 218 }
 219
 220 void Robot::get_est_pos(double &x, double &y, double &phi) {
 221         if (indep_odometry_works) {
 222                 Guard g(lock_est_pos_indep_odo);
 223                 x = est_pos_indep_odo.x;
 224                 y = est_pos_indep_odo.y;
 225                 phi = est_pos_indep_odo.phi;
 226         } else if (odometry_works) {
 227                 Guard g(lock_est_pos_odo);
 228                 x = est_pos_odo.x;
 229                 y = est_pos_odo.y;
 230                 phi = est_pos_odo.phi;
 231         } else {
 232                 Guard g(lock_ref_pos);
 233                 x = ref_pos.x;
 234                 y = ref_pos.y;
 235                 phi = ref_pos.phi;
 236         }
 237 }
 238
 239 /** Sets actual position of the robot and with respoect to color of
 240  * the team. Should be used for setting initial position of the
 241  * robot. */
 242 void Robot::set_est_pos(double x, double y, double phi) {
 243         if (x<0) x=0;
 244         if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
 245         if (y<0) y=0;
 246         if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
 247
 248         {
 249                 Guard g(lock_est_pos_indep_odo);
 250                 est_pos_indep_odo.x = x;
 251                 est_pos_indep_odo.y = y;
 252                 est_pos_indep_odo.phi = phi;
 253         }
 254
 255         {
 256                 Guard g(lock_est_pos_odo);
 257                 est_pos_odo.x = x;
 258                 est_pos_odo.y = y;
 259                 est_pos_odo.phi = phi;
 260         }
 261
 262         {
 263                 Guard g(lock_ref_pos);
 264                 ref_pos.x = x;
 265                 ref_pos.y = y;
 266                 ref_pos.phi = phi;
 267         }
 268 }
 269
 270 float Robot::current_time() {
 271         timespec now, diff, start_local;
 272         start_local = startTime;
 273         clock_gettime(CLOCK_MONOTONIC, &now);
 274         timespec_subtract(diff, now, start_local);
 275         return diff.tv_sec + diff.tv_nsec/1000000000.0;
 276 }