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