src/robofsm/motion-control.cc

   1 /**
   2  * @file   motion-control.cc
   3  * @author Michal Sojka <sojkam1@fel.cvut.cz>, Petr Beneš
   4  * @date   Fri Mar 20 10:36:59 2009
   5  *
   6  * @brief
   7  *
   8  *
   9  */
  10
  11 //#define MOTION_DEBUG
  12
  13 #ifdef MOTION_DEBUG
  14     #define DBG(format, ...) printf(format, ##__VA_ARGS__)
  15     #define DBGflush() fflush(stdout)
  16 #else
  17     #define DBG(format, ...)
  18     #define DBGflush()
  19 #endif
  20
  21 #include <sys/time.h>
  22 #include <time.h>
  23 #include "trgen.h"
  24 #include "balet.h"
  25 #include <robodata.h>
  26 #include <robot.h>
  27 #include <pthread.h>
  28 #include <path_planner.h>
  29 #include <signal.h>
  30 #include <movehelper.h>
  31 #include <sharp.h>
  32 #include <unistd.h>
  33 #include <map.h>
  34 #include "robot_config.h"
  35 #include <robomath.h>
  36
  37 #define MOTION_CONTROL
  38 #include "motion-control.h"
  39
  40 /* ULoPoS EKF */
  41 #include <ekf.h>
  42 #include <geom.h>
  43
  44 /* ULoPoS constants (-%-TEMPERATURE-%- dependent!) */
  45 #define SOUND_VELOCITY   (331.3+0.606*20)
  46 #define XCORR2METER      (SOUND_VELOCITY*(127.0/508.0)/3000.0)
  47 #define D_MAX            (XCORR2METER*508.0)
  48
  49 /*******************************************************************************
  50  * Controller thread and helper functions for that thread
  51  *******************************************************************************/
  52
  53 /**
  54  * If the distance of robot's estimated position from robot's
  55  * requested position if above this value, the robot lost and we try
  56  * to reset localization.
  57  */
  58 #define MAX_POS_ERROR_M 0.25
  59
  60 /**
  61  * If trajectory end is reached and robot's estimated position is
  62  * closer than this distance, the movement is considered as "done".
  63  */
  64 #define CLOSE_TO_TARGET_M 0.1
  65
  66 //Controller gains
  67 const struct balet_params k = {
  68   p_tangent:  3,                // dx gain
  69   p_angle: 2,                   // dphi gain
  70   p_perpen: 5                   // dy gain
  71 //   p_tangent:  0.2,           // dx gain
  72 //   p_angle: 0.15,                     // dphi gain
  73 //   p_perpen: 1                        // dy gain
  74 };
  75
  76 #define MOTION_PERIOD_NS (50/*ms*/*1000*1000)
  77 #define MEASURE_TIMEOUT_NS (100/*ms*/*1000*1000)
  78
  79 #define SIG_DO_CONTROL_NOW (SIGRTMIN+1)
  80
  81 // Global varibles
  82 static pthread_t thr_trajectory_follower;
  83 static struct timeval tv_start; /**< Absolute time, when trajectory started. */
  84
  85 /** Stores the actually followed trajectory object */
  86 static Trajectory *actual_trajectory;
  87 static pthread_mutex_t actual_trajectory_lock;
  88
  89 // Trajectory recalculation
  90 sem_t recalculation_not_running;
  91 sem_t measurement_received;
  92
  93 /**
  94  * Determines way of thread_trajectory_follower() operation:
  95  * - 0 measurement doesn't work, controller invocation based on time (formerly CONFIG_OPEN_LOOP)
  96  * - 2 measurement works, controller invocation based on sem_post
  97  * - 1 measurement doesn't work and stop() was called
  98  */
  99 int measurement_ok = 0;
 100
 101
 102
 103 static void delete_actual_trajectory()
 104 {
 105         Trajectory *old;
 106         pthread_mutex_lock(&actual_trajectory_lock);
 107         old = actual_trajectory;
 108         actual_trajectory = NULL;
 109         pthread_mutex_unlock(&actual_trajectory_lock);
 110         robot_send_speed(0,0);
 111         if (old) delete(actual_trajectory);
 112 }
 113
 114 /** Sends events from follower thread to FSM. */
 115 static void notify_fsm(bool done, double error)
 116 {
 117         static bool done_sent;
 118         static bool lost_sent = false;
 119
 120         if (error > MAX_POS_ERROR_M) {
 121                 if (!lost_sent) {
 122                         lost_sent = true;
 123                         FSM_SIGNAL(MOTION, EV_TRAJECTORY_LOST, NULL);
 124                 }
 125         } else {
 126                 lost_sent = false;
 127                 if (done) {
 128                         if (error < CLOSE_TO_TARGET_M) {
 129                                 FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE_AND_CLOSE, NULL);
 130                         } else if (!done_sent) {
 131                                 done_sent = true;
 132                                 FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE, NULL);
 133                         }
 134                 } else {
 135                         done_sent = false;
 136                 }
 137         }
 138 }
 139
 140 static void check_for_collision_in_future(Trajectory *traj, double current_time)
 141 {
 142         Pos future_pos;
 143         struct map *map = robot.map;
 144         int xcell, ycell;
 145         double x, y;
 146         bool valid;
 147         unsigned i;
 148 //      const double times[] = { 0.5, 0.3, 0.1 }; // seconds
 149         const double times[] = { 0.3, 0.4, 0.5, 0.7, 0.9, 1.1 }; // seconds
 150
 151
 152         for (i=0; i < sizeof(times)/sizeof(times[0]); i++) {
 153                 traj->getRefPos(current_time+times[i], future_pos);
 154
 155                 /* Ignore obstacles when turning */
 156                 if (fabs(future_pos.v) < 0.01)
 157                         continue;
 158
 159                 x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
 160                 y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
 161
 162                 ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
 163                 if (!valid)
 164                         continue;
 165                 if (map->cells[ycell][xcell].detected_obstacle > 0) {
 166                         if (sem_trywait(&recalculation_not_running) == 0) {
 167                                 FSM_SIGNAL(MOTION, EV_OBSTACLE, NULL);
 168                                 break;
 169                         }
 170                 }
 171         }
 172 }
 173
 174 static void do_estimation()
 175 {
 176         static bool virgo = true;
 177         static real_t beacon_xy[3][2] = {
 178                 { 3.062, -0.05},
 179                 {-0.062,  1.05},
 180                 { 3.062,  2.162},
 181         };
 182         static ekf8_t ekf8;
 183         static uint32_t odo0[2];
 184         static real_t y[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
 185         static int missing_odo_count = 0;
 186         uint32_t t[3], odo[2];
 187         real_t x[8], P[8*8];
 188         int i, odo_received, err[5];
 189
 190         /* locks should not be necessary, however... */
 191         ROBOT_LOCK(corr_distances);
 192         t[0] = robot.corr_distances.t1;
 193         t[1] = robot.corr_distances.t2;
 194         t[2] = robot.corr_distances.t3;
 195         ROBOT_UNLOCK(corr_distances);
 196         ROBOT_LOCK(motion_irc);
 197         odo[0] = robot.motion_irc.left;
 198         odo[1] = robot.motion_irc.right;
 199         odo_received = robot.motion_irc_received;
 200         robot.motion_irc_received = 0;
 201         ROBOT_UNLOCK(motion_irc);
 202
 203         for (i = 0; i < 3; i++)
 204                 y[i] = (XCORR2METER/32.0)*t[i];
 205
 206         /* missing odometry workaround :-( */
 207         if (odo_received) {
 208                 real_t c = 1.0/(real_t)(1 + missing_odo_count);
 209                 y[3] = c*ODO_C*(real_t)((int32_t)(odo0[0] - odo[0]));
 210                 y[4] = c*ODO_C*(real_t)((int32_t)(odo[1] - odo0[1]));
 211                 missing_odo_count = 0;
 212         }
 213         else
 214                 ++missing_odo_count;
 215
 216         odo0[0] = odo[0];  odo0[1] = odo[1];
 217         DBG("UZV+ODO:  %f  %f  %f  %f  %f\n", y[0], y[1], y[2], y[3], y[4]);
 218         //DBG("ODO:  %f  %f    %u  %u\n", y[3], y[4], odo[0], odo[1]);
 219
 220         if (virgo || init_ekf_flag) {
 221                 /*FIXME:reflect init pos & beacon coords accord.to our color */
 222                 ROBOT_LOCK(est_pos);
 223                 real_t xy0[] = {robot.est_pos.x, robot.est_pos.y};
 224                 y[3] = y[4] = 0.0;
 225                 ekf8_init(&ekf8, (real_t*)beacon_xy, D_MAX, 30.0, xy0, y);
 226                 ekf8.ekf.x[6] = robot.est_pos.phi;
 227                 init_ekf_flag = false;
 228                 virgo = false;
 229                 ROBOT_UNLOCK(est_pos);
 230         }
 231
 232         ekf8_step(&ekf8, x, P, err, y);
 233
 234         DBG("EKF: x=%f   y=%f   phi=%8.4f\n", x[0], x[1], x[6]*(180.0/M_PI));
 235
 236         ROBOT_LOCK(est_pos);
 237         robot.est_pos.x = x[0] - ODO_D*cos(x[6]);
 238         robot.est_pos.y = x[1] - ODO_D*sin(x[6]);
 239         robot.est_pos.phi = x[6];
 240         ROBOT_UNLOCK(est_pos);
 241 }
 242
 243 static void do_control()
 244 {
 245         double speedl, speedr;
 246
 247         double t;
 248         struct timeval tv;
 249
 250         // Calculate reference position
 251         /***FIXME:should not rely on system clock, the period is fixed***/
 252         gettimeofday(&tv, NULL);
 253         t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 254         t += (tv.tv_sec - tv_start.tv_sec);
 255 /*
 256         // check for new trajectory to switch
 257         // only if the trajectory is already prepared
 258         if (switch_to_trajectory != NULL && t >= switch_time) {
 259                 pthread_mutex_lock(&switch_to_trajectory_lock);
 260
 261                 DBG("SWITCHING to new trajectory\n");
 262
 263                 go(switch_to_trajectory);
 264                 // nothing prepared now
 265                 switch_to_trajectory = NULL;
 266                 pthread_mutex_unlock(&switch_to_trajectory_lock);
 267         }
 268 */
 269         pthread_mutex_lock(&actual_trajectory_lock);
 270         Trajectory *w = actual_trajectory;
 271         if (w) {
 272                 Pos ref_pos, est_pos, balet_out;
 273                 bool done;
 274
 275                 // Calculate reference position
 276                 gettimeofday(&tv, NULL);
 277                 t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 278                 t += (tv.tv_sec - tv_start.tv_sec);
 279
 280                 // if switch_to_trajectory is being prepared, it can not stop calculation
 281                 // and start to count again, it could evoke overloading
 282                 if (robot.obstacle_avoidance_enabled)
 283                         check_for_collision_in_future(w, t);
 284
 285
 286                 done = w->getRefPos(t, ref_pos);
 287
 288                 if (ref_pos.omega > actual_trajectory->constr.maxomega)
 289                         DBG("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, actual_trajectory->constr.maxomega);
 290
 291                 ROBOT_LOCK(ref_pos);
 292                 robot.ref_pos.x = ref_pos.x;
 293                 robot.ref_pos.y = ref_pos.y;
 294                 robot.ref_pos.phi = ref_pos.phi;
 295                 ROBOT_UNLOCK(ref_pos);
 296
 297                 ROBOT_LOCK(est_pos);
 298                 est_pos.x = robot.est_pos.x;
 299                 est_pos.y = robot.est_pos.y;
 300                 est_pos.phi = robot.est_pos.phi;
 301                 ROBOT_UNLOCK(est_pos);
 302
 303                 if (measurement_ok < 2) {
 304                         // We don't have any feedback now. It is
 305                         // supposed that the estimated position is
 306                         // equal to the reference position.
 307                         robot_set_est_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
 308                         est_pos.x = ref_pos.x;
 309                         est_pos.y = ref_pos.y;
 310                         est_pos.phi = ref_pos.phi;
 311                 }
 312
 313 #ifdef MOTION_PRINT_REF
 314                 static double last_t;
 315                 if (t < last_t) last_t = t; // Switched to a new trajectory
 316                 printf("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f  omega=%-4.02f, time=%lf dt=%lf \n", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega, t, t-last_t);
 317                 fflush(stdout);
 318                 last_t = t;
 319 #endif
 320
 321                 // Call the controller
 322                 double error;
 323                 error = balet(ref_pos, est_pos, k, balet_out);
 324                 speedl = balet_out.v - ROBOT_ROTATION_RADIUS_M*balet_out.omega;
 325                 speedr = balet_out.v + ROBOT_ROTATION_RADIUS_M*balet_out.omega;
 326                 notify_fsm(done, error);
 327         } else {
 328                 speedl = 0;
 329                 speedr = 0;
 330         }
 331
 332
 333         // Apply controller output
 334         robot_send_speed(speedl, speedr);
 335         pthread_mutex_unlock(&actual_trajectory_lock);
 336 }
 337
 338 void dummy_handler(int)
 339 {
 340 }
 341
 342 static inline void next_period(struct timespec *next, long long interval_ns)
 343 {
 344         next->tv_nsec += interval_ns;
 345         if (next->tv_nsec >= 1000000000) {
 346                 next->tv_sec++;
 347                 next->tv_nsec -= 1000000000;
 348         }
 349 }
 350
 351 /**
 352  * A thread running the controller.
 353  *
 354  * This (high priority) thread executes the motion control
 355  * algorithm. It calculates repference position based on actual
 356  * trajectory and current time. Then it calls "balet" controller to
 357  * close feedback.
 358  *
 359  * @param arg
 360  *
 361  * @return
 362  */
 363 void *thread_trajectory_follower(void *arg)
 364 {
 365         struct timespec next;
 366         int ret;
 367
 368         clock_gettime(CLOCK_REALTIME, &next);
 369
 370         while (1) {
 371                 ret = sem_timedwait(&measurement_received, &next);
 372
 373                 if (ret == -1 && errno == ETIMEDOUT) {
 374                         next_period(&next, MOTION_PERIOD_NS);
 375                         if (measurement_ok) {
 376                                 if (measurement_ok == 2) {
 377                                         fprintf(stderr, "problem: measurement timeout!!!!!!!!!!!");
 378                                 }
 379                                 measurement_ok--;
 380                         }
 381
 382
 383                 } else {
 384                         next_period(&next, MEASURE_TIMEOUT_NS);
 385                         if (measurement_ok < 2) {
 386                                 measurement_ok++;
 387                         }
 388                 }
 389
 390                 robot.localization_works = (measurement_ok == 2);
 391                 if (measurement_ok == 2) {
 392                         do_estimation();
 393                 }
 394                 do_control();
 395
 396         }
 397 }
 398
 399 /**
 400  * Tells trajctory_follower to start moving along trajectory @c t.
 401  *
 402  * @param t Trajectory to follow.
 403  * @param append_time Relative time from the beginning of the @c actual_trajectory
 404  * when to append the new one
 405  */
 406 void go(Trajectory *t, double append_time)
 407 {
 408         pthread_mutex_lock(&actual_trajectory_lock);
 409         Trajectory *old;
 410         if (actual_trajectory && append_time != 0) {
 411                 // trajectory only connects a new one in some specific time
 412                 if(!actual_trajectory->appendTrajectory(*t, append_time))
 413                         DBG("Can not append trajectory\n");
 414         } else {
 415                 // trajectory starts from zero time
 416                 old = actual_trajectory;
 417                 gettimeofday(&tv_start, NULL);
 418                 actual_trajectory = t;
 419 #ifdef MOTION_LOG
 420                 t->logTraj(tv_start.tv_sec + 1e-6*tv_start.tv_usec);
 421 #endif
 422                 if (old)
 423                         delete(old);
 424         }
 425         pthread_mutex_unlock(&actual_trajectory_lock);
 426 }
 427
 428 /**
 429  * switches to newly calculated trajectory to go on it at specific time
 430  */
 431 /*void switch_trajectory_at(Trajectory *t, double time)
 432 {
 433         pthread_mutex_lock(&switch_to_trajectory_lock);
 434         switch_to_trajectory = t;
 435         switch_time = time;
 436         pthread_mutex_unlock(&switch_to_trajectory_lock);
 437
 438         struct timeval tv;
 439         gettimeofday(&tv, NULL);
 440         double tm = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 441         tm += (tv.tv_sec - tv_start.tv_sec);
 442         if (switch_time <= tm)
 443                 DBG("//// BAD SWITCH ////");
 444 }
 445 */
 446 void stop()
 447 {
 448         delete_actual_trajectory();
 449
 450         // Interrupt sem_timedwait() in thread_trajectory_follower(),
 451         // so we stop immediately.
 452         sem_post(&measurement_received);
 453 }
 454
 455 /**
 456  * Initializes motion controller.
 457  *
 458  *
 459  * @return Zero on success, non-zero otherwise.
 460  */
 461 int motion_control_init()
 462 {
 463         pthread_attr_t tattr;
 464         sched_param param;
 465         pthread_mutexattr_t mattr;
 466         int ret;
 467
 468         actual_trajectory = NULL;
 469         //switch_to_trajectory = NULL;
 470
 471
 472         ret = pthread_mutexattr_init(&mattr);
 473 #ifdef HAVE_PRIO_INHERIT
 474         ret = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
 475 #endif
 476         pthread_mutex_init(&actual_trajectory_lock, &mattr);
 477
 478         sem_init(&recalculation_not_running, 0, 1);
 479
 480         // Trajectory follower thread
 481         sem_init(&measurement_received, 0, 0);
 482         pthread_attr_init (&tattr);
 483         pthread_attr_getschedparam (&tattr, &param);
 484         pthread_attr_setschedpolicy(&tattr, SCHED_FIFO);
 485         param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER;
 486         ret = pthread_attr_setschedparam (&tattr, &param);
 487         if(ret) {
 488                 perror("move_init: pthread_attr_setschedparam(follower)");
 489                 goto err;
 490         }
 491         ret = pthread_create(&thr_trajectory_follower, &tattr, thread_trajectory_follower, NULL);
 492         if(ret) {
 493                 perror("move_init: pthread_create");
 494                 goto err;
 495         }
 496
 497         return 0;
 498   err:
 499         return ret;
 500 }
 501
 502 void motion_control_done()
 503 {
 504         pthread_cancel(thr_trajectory_follower);
 505         pthread_join(thr_trajectory_follower, NULL);
 506
 507         robot.orte.motion_speed.right = 0;
 508         robot.orte.motion_speed.left = 0;
 509         ORTEPublicationSend(robot.orte.publication_motion_speed);
 510 }
 511
 512
 513 void get_future_pos(double rel_time_sec, Pos &pos, double &switch_time)
 514 {
 515         struct timeval tv;
 516
 517         gettimeofday(&tv, NULL);
 518         switch_time = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 519         switch_time += (tv.tv_sec - tv_start.tv_sec);
 520         switch_time += rel_time_sec;
 521
 522         pthread_mutex_lock(&actual_trajectory_lock);
 523         if (actual_trajectory) {
 524                 actual_trajectory->getRefPos(switch_time, pos);
 525                 pthread_mutex_unlock(&actual_trajectory_lock);
 526         } else {
 527                 // Robot doesn't move, so return current position
 528                 pthread_mutex_unlock(&actual_trajectory_lock);
 529
 530                 ROBOT_LOCK(est_pos);
 531                 pos.x = robot.est_pos.x;
 532                 pos.y = robot.est_pos.y;
 533                 pos.phi = robot.est_pos.phi;
 534                 pos.v = 0;
 535                 pos.omega = 0;
 536                 ROBOT_UNLOCK(est_pos);
 537         }
 538 }