src/robofsm/robot.cc

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