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