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, ...) ul_logdeb(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 <ul_log.h>
  37
  38 UL_LOG_CUST(ulogd_motion_control); /* Log domain name = ulogd + name of the file */
  39
  40 #define MOTION_CONTROL
  41 #include "motion-control.h"
  42
  43 /* ULoPoS EKF */
  44 #include <ekf.h>
  45 #include <geom.h>
  46
  47 /* ULoPoS constants (-%-TEMPERATURE-%- dependent!) */
  48 #define SOUND_VELOCITY   (331.3+0.606*20)
  49 #define XCORR2METER      (SOUND_VELOCITY*(127.0/508.0)/3000.0)
  50 #define D_MAX            (XCORR2METER*508.0)
  51
  52 /*******************************************************************************
  53  * Controller thread and helper functions for that thread
  54  *******************************************************************************/
  55
  56 /**
  57  * If the distance of robot's estimated position from robot's
  58  * requested position if above this value, the robot lost and we try
  59  * to reset localization.
  60  */
  61 #define MAX_POS_ERROR_M 0.25
  62
  63 /**
  64  * If trajectory end is reached and robot's estimated position is
  65  * closer than this distance, the movement is considered as "done".
  66  */
  67 #define CLOSE_TO_TARGET_M 0.1
  68
  69 //Controller gains
  70 const struct balet_params k = {
  71         //p_tangent:  3,        // dx gain
  72         //p_angle: 2,           // dphi gain
  73         //p_perpen: 5           // dy gain
  74         p_tangent:  0.2,        // dx gain
  75         p_angle: 0.15,          // dphi gain
  76         p_perpen: 1             // dy gain
  77 };
  78
  79 #define MOTION_PERIOD_NS (50/*ms*/*1000*1000)
  80 #define MEASURE_TIMEOUT_NS (100/*ms*/*1000*1000)
  81
  82 #define SIG_DO_CONTROL_NOW (SIGRTMIN+1)
  83
  84 // Global varibles
  85 static pthread_t thr_trajectory_follower;
  86 static struct timeval tv_start; /**< Absolute time, when trajectory started. */
  87
  88 /** Stores the actually followed trajectory object */
  89 static Trajectory *actual_trajectory;
  90 static pthread_mutex_t actual_trajectory_lock;
  91
  92 // Trajectory recalculation
  93 sem_t recalculation_not_running;
  94 sem_t measurement_received;
  95
  96 /**
  97  * Determines way of thread_trajectory_follower() operation:
  98  * - 0 measurement doesn't work, controller invocation based on time (formerly CONFIG_OPEN_LOOP)
  99  * - 2 measurement works, controller invocation based on sem_post
 100  * - 1 measurement doesn't work and stop() was called
 101  */
 102 int measurement_ok = 0;
 103
 104
 105
 106 static void delete_actual_trajectory()
 107 {
 108         Trajectory *old;
 109         pthread_mutex_lock(&actual_trajectory_lock);
 110         old = actual_trajectory;
 111         actual_trajectory = NULL;
 112         pthread_mutex_unlock(&actual_trajectory_lock);
 113         robot_send_speed(0,0);
 114         if (old) delete(old);
 115 }
 116
 117 /** Sends events from follower thread to FSM. */
 118 static void notify_fsm(bool done, double error)
 119 {
 120         static bool done_sent;
 121         static bool lost_sent = false;
 122
 123         if (error > MAX_POS_ERROR_M) {
 124                 if (!lost_sent) {
 125                         lost_sent = true;
 126                         FSM_SIGNAL(MOTION, EV_TRAJECTORY_LOST, NULL);
 127                 }
 128         } else {
 129                 lost_sent = false;
 130                 if (done) {
 131                         if (error < CLOSE_TO_TARGET_M) {
 132                                 FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE_AND_CLOSE, NULL);
 133                         } else if (!done_sent) {
 134                                 done_sent = true;
 135                                 FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE, NULL);
 136                         }
 137                 } else {
 138                         done_sent = false;
 139                 }
 140         }
 141 }
 142
 143 static void check_for_collision_in_future(Trajectory *traj, double current_time)
 144 {
 145         Pos future_pos;
 146         struct map *map = robot.map;
 147         int xcell, ycell;
 148         double x, y;
 149         bool valid;
 150         unsigned i;
 151         const double times[] = { 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.5, 2.0 }; // seconds
 152
 153
 154         for (i=0; i < sizeof(times)/sizeof(times[0]); i++) {
 155                 traj->getRefPos(current_time+times[i], future_pos);
 156
 157                 /* Ignore obstacles when turning */
 158                 if (fabs(future_pos.v) < 0.01)
 159                         continue;
 160
 161                 x = future_pos.x /*+ cos(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M */;
 162                 y = future_pos.y /*+ sin(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M */;
 163
 164                 ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
 165                 if (!valid)
 166                         continue;
 167                 if (map->cells[ycell][xcell].detected_obstacle > 0) {
 168                         if (sem_trywait(&recalculation_not_running) == 0) {
 169                                 FSM_SIGNAL(MOTION, EV_OBSTACLE, NULL);
 170                                 break;
 171                         }
 172                 }
 173         }
 174 }
 175
 176 static void do_control()
 177 {
 178         double speedl, speedr;
 179
 180         double t;
 181         struct timeval tv;
 182
 183         // Calculate reference position
 184         /***FIXME:should not rely on system clock, the period is fixed***/
 185         gettimeofday(&tv, NULL);
 186         t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 187         t += (tv.tv_sec - tv_start.tv_sec);
 188 /*
 189         // check for new trajectory to switch
 190         // only if the trajectory is already prepared
 191         if (switch_to_trajectory != NULL && t >= switch_time) {
 192                 pthread_mutex_lock(&switch_to_trajectory_lock);
 193
 194                 DBG("SWITCHING to new trajectory\n");
 195
 196                 go(switch_to_trajectory);
 197                 // nothing prepared now
 198                 switch_to_trajectory = NULL;
 199                 pthread_mutex_unlock(&switch_to_trajectory_lock);
 200         }
 201 */
 202         pthread_mutex_lock(&actual_trajectory_lock);
 203         Trajectory *w = actual_trajectory;
 204         if (w) {
 205                 Pos ref_pos, est_pos, balet_out;
 206                 bool done;
 207
 208                 // Calculate reference position
 209                 gettimeofday(&tv, NULL);
 210                 t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 211                 t += (tv.tv_sec - tv_start.tv_sec);
 212
 213                 // if switch_to_trajectory is being prepared, it can not stop calculation
 214                 // and start to count again, it could evoke overloading
 215                 if (robot.obstacle_avoidance_enabled)
 216                         check_for_collision_in_future(w, t);
 217
 218
 219                 done = w->getRefPos(t, ref_pos);
 220
 221                 if (ref_pos.omega > actual_trajectory->constr.maxomega)
 222                         DBG("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, actual_trajectory->constr.maxomega);
 223
 224                 ROBOT_LOCK(ref_pos);
 225                 robot.ref_pos.x = ref_pos.x;
 226                 robot.ref_pos.y = ref_pos.y;
 227                 robot.ref_pos.phi = ref_pos.phi;
 228                 ROBOT_UNLOCK(ref_pos);
 229
 230                 robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
 231
 232 #ifdef MOTION_PRINT_REF
 233                 static double last_t;
 234                 if (t < last_t) last_t = t; // Switched to a new trajectory
 235                 ul_logdeb("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);
 236                 fflush(stdout);
 237                 last_t = t;
 238 #endif
 239
 240                 // Call the controller
 241                 double error;
 242                 error = balet(ref_pos, est_pos, k, balet_out);
 243                 speedl = balet_out.v - ROBOT_ROTATION_RADIUS_M*balet_out.omega;
 244                 speedr = balet_out.v + ROBOT_ROTATION_RADIUS_M*balet_out.omega;
 245                 notify_fsm(done, error);
 246         } else {
 247                 speedl = 0;
 248                 speedr = 0;
 249         }
 250
 251
 252         // Apply controller output
 253         robot_send_speed(speedl, speedr);
 254         pthread_mutex_unlock(&actual_trajectory_lock);
 255 }
 256
 257 static inline void next_period(struct timespec *next, long long interval_ns)
 258 {
 259         next->tv_nsec += interval_ns;
 260         if (next->tv_nsec >= 1000000000) {
 261                 next->tv_sec++;
 262                 next->tv_nsec -= 1000000000;
 263         }
 264 }
 265
 266 /**
 267  * A thread running the controller.
 268  *
 269  * This (high priority) thread executes the motion control
 270  * algorithm. It calculates repference position based on actual
 271  * trajectory and current time. Then it calls "balet" controller to
 272  * close feedback.
 273  *
 274  * @param arg
 275  *
 276  * @return
 277  */
 278 void *thread_trajectory_follower(void *arg)
 279 {
 280         struct timespec next;
 281         int ret;
 282         struct sched_param param;
 283
 284         param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER;
 285         if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &param) != 0)
 286                 perror("Warning: Cannot set RT priority for thread_prio_traj_follower()");
 287
 288         clock_gettime(CLOCK_REALTIME, &next);
 289
 290         while (1) {
 291                 ret = sem_timedwait(&measurement_received, &next);
 292
 293                 if (ret == -1 && errno == ETIMEDOUT) {
 294                         next_period(&next, MOTION_PERIOD_NS);
 295                         if (measurement_ok) {
 296                                 if (measurement_ok == 2) {
 297                                         fprintf(stderr, "problem: measurement timeout!!!!!!!!!!!\n");
 298                                 }
 299                                 measurement_ok--;
 300                         }
 301                 } else {
 302                         next_period(&next, MEASURE_TIMEOUT_NS);
 303                         if (measurement_ok < 2) {
 304                                 measurement_ok++;
 305                         }
 306                 }
 307                 do_control();
 308         }
 309         return NULL;
 310 }
 311
 312 /**
 313  * Tells trajctory_follower to start moving along trajectory @c t.
 314  *
 315  * @param t Trajectory to follow.
 316  * @param append_time Relative time from the beginning of the @c actual_trajectory
 317  * when to append the new one
 318  */
 319 void go(Trajectory *t, double append_time)
 320 {
 321         pthread_mutex_lock(&actual_trajectory_lock);
 322         Trajectory *old;
 323         if (actual_trajectory && append_time != 0) {
 324                 // trajectory only connects a new one in some specific time
 325                 if(!actual_trajectory->appendTrajectory(*t, append_time))
 326                         DBG("Can not append trajectory\n");
 327         } else {
 328                 // trajectory starts from zero time
 329                 old = actual_trajectory;
 330                 gettimeofday(&tv_start, NULL);
 331                 actual_trajectory = t;
 332 #ifdef MOTION_LOG
 333                 t->logTraj(tv_start.tv_sec + 1e-6*tv_start.tv_usec);
 334 #endif
 335                 if (old)
 336                         delete(old);
 337         }
 338         pthread_mutex_unlock(&actual_trajectory_lock);
 339 }
 340
 341 /**
 342  * switches to newly calculated trajectory to go on it at specific time
 343  */
 344 /*void switch_trajectory_at(Trajectory *t, double time)
 345 {
 346         pthread_mutex_lock(&switch_to_trajectory_lock);
 347         switch_to_trajectory = t;
 348         switch_time = time;
 349         pthread_mutex_unlock(&switch_to_trajectory_lock);
 350
 351         struct timeval tv;
 352         gettimeofday(&tv, NULL);
 353         double tm = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 354         tm += (tv.tv_sec - tv_start.tv_sec);
 355         if (switch_time <= tm)
 356                 DBG("//// BAD SWITCH ////");
 357 }
 358 */
 359 void stop()
 360 {
 361         delete_actual_trajectory();
 362
 363         // Interrupt sem_timedwait() in thread_trajectory_follower(),
 364         // so we stop immediately.
 365         sem_post(&measurement_received);
 366 }
 367
 368 /**
 369  * Initializes motion controller.
 370  *
 371  *
 372  * @return Zero on success, non-zero otherwise.
 373  */
 374 int motion_control_init()
 375 {
 376         pthread_mutexattr_t mattr;
 377         int ret;
 378
 379         actual_trajectory = NULL;
 380         //switch_to_trajectory = NULL;
 381
 382
 383         ret = pthread_mutexattr_init(&mattr);
 384 #ifdef HAVE_PRIO_INHERIT
 385         ret = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
 386 #endif
 387         pthread_mutex_init(&actual_trajectory_lock, &mattr);
 388
 389         sem_init(&recalculation_not_running, 0, 1);
 390
 391         // Trajectory follower thread
 392         sem_init(&measurement_received, 0, 0);
 393         ret = pthread_create(&thr_trajectory_follower, NULL, thread_trajectory_follower, NULL);
 394         if(ret) {
 395                 perror("move_init: pthread_create");
 396                 goto err;
 397         }
 398
 399         return 0;
 400   err:
 401         return ret;
 402 }
 403
 404 void motion_control_done()
 405 {
 406         pthread_cancel(thr_trajectory_follower);
 407         pthread_join(thr_trajectory_follower, NULL);
 408
 409         robot.orte.motion_speed.right = 0;
 410         robot.orte.motion_speed.left = 0;
 411         ORTEPublicationSend(robot.orte.publication_motion_speed);
 412 }
 413
 414
 415 void get_future_pos(double rel_time_sec, Pos &pos, double &switch_time)
 416 {
 417         struct timeval tv;
 418
 419         gettimeofday(&tv, NULL);
 420         switch_time = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 421         switch_time += (tv.tv_sec - tv_start.tv_sec);
 422         switch_time += rel_time_sec;
 423
 424         pthread_mutex_lock(&actual_trajectory_lock);
 425         if (actual_trajectory) {
 426                 actual_trajectory->getRefPos(switch_time, pos);
 427                 pthread_mutex_unlock(&actual_trajectory_lock);
 428         } else {
 429                 // Robot doesn't move, so return current position
 430                 pthread_mutex_unlock(&actual_trajectory_lock);
 431
 432                 robot_get_est_pos(&pos.x, &pos.y, &pos.phi);
 433                 pos.v = 0;
 434                 pos.omega = 0;
 435         }
 436 }