src/robofsm/eb2008/fsmmove.cc

   1 /**
   2  * @file        fsmmove.cc
   3  * @brief       FSM move
   4  * @author      Michal Sojka, Jose Maria Martin Laguna
   5  *
   6  */
   7 #define DEBUG
   8 #define FSM_MOTION
   9 #include <sys/time.h>
  10 #include <time.h>
  11 #include "trgen.h"
  12 #include "balet.h"
  13 #include <robodata.h>
  14 #include <robot_eb2008.h>
  15 #include <pthread.h>
  16 #include <path_planner.h>
  17 #include <signal.h>
  18 #include <movehelper_eb2008.h>
  19 #include <sharp.h>
  20 #include <unistd.h>
  21 #include <map.h>
  22 #include "robot_config.h"
  23 #include <robomath.h>
  24
  25 /*******************************************************************************
  26  * Controller thread and helper functions for that thread
  27  *******************************************************************************/
  28
  29
  30 //Controller gains
  31 const struct balet_params k = {
  32   p_tangent:  2,                // dx gain
  33   p_angle: 2,                   // dphi gain
  34   p_perpen: 10                  // dy gain
  35 };
  36
  37 #define MAX_POS_ERROR_M 0.5
  38 #define CLOSE_TO_TARGET_M 0.1
  39 #define MAX_WAIT_FOR_CLOSE_MS 2000
  40
  41 //#define MOTION_PERIOD_NS (500/*ms*/*1000*1000)
  42 #define MOTION_PERIOD_NS (50/*ms*/*1000*1000)
  43 #define SIG_TIMER (SIGRTMIN+0)
  44 #define SIG_NEWDATA (SIGRTMIN+1)
  45
  46 // Global varibles
  47 static pthread_t thr_trajectory_follower;
  48 static struct timeval tv_start; /**< Absolute time, when trajectory started. */
  49
  50 // Trajectory recalculation thread and synchoronization
  51 pthread_t thr_trajctory_recalculation;
  52 sem_t start_recalculation;
  53 sem_t recalculation_not_running;
  54
  55 /** Stores the actually followed trajectory object */
  56 static Trajectory *actual_trajectory = NULL;
  57 bool actual_trajectory_reverse;
  58 pthread_mutex_t actual_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
  59 pthread_mutex_t switch_to_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
  60
  61
  62 // Target position of the current movement
  63 static struct move_target current_target;
  64
  65
  66 //static Trajectory *switch_to_trajectory = NULL;
  67 //static double switch_time;
  68
  69 static void stop();
  70
  71 static void delete_actual_trajectory()
  72 {
  73         Trajectory *old;
  74         pthread_mutex_lock(&actual_trajectory_lock);
  75         old = actual_trajectory;
  76         actual_trajectory = NULL;
  77         pthread_mutex_unlock(&actual_trajectory_lock);
  78         robot_send_speed(0,0);
  79         if (old) delete(actual_trajectory);
  80 }
  81
  82 /** Sends events from follower thread to FSM. */
  83 static void notify_fsm(bool done, double error)
  84 {
  85         static bool done_sent;
  86
  87         if (error > MAX_POS_ERROR_M) {
  88             FSM_SIGNAL(MOTION, EV_TRAJECTORY_LOST, NULL);
  89         } else {
  90                 if (done) {
  91                         if (error < CLOSE_TO_TARGET_M) {
  92                                 FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE_AND_CLOSE, NULL);
  93                         } else if (!done_sent) {
  94                                 done_sent = true;
  95                                 FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE, NULL);
  96                         }
  97                 } else {
  98                         done_sent = false;
  99                 }
 100         }
 101 }
 102
 103 static void check_for_collision_in_future(Trajectory *traj, double current_time)
 104 {
 105         Pos future_pos;
 106         struct map *map = robot.map;
 107         int xcell, ycell;
 108         double x, y;
 109         bool valid;
 110
 111         traj->getRefPos(current_time+0.5/*s*/, future_pos);
 112
 113         /* Ignore obstacles when turning */
 114         if (fabs(future_pos.v) < 0.01)
 115                 return;
 116
 117         x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
 118         y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
 119
 120         ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
 121         if (!valid)
 122                 return;
 123         if (map->cells[ycell][xcell].detected_obstacle > 0) {
 124                 if (sem_trywait(&recalculation_not_running) == 0) {
 125                         sem_post(&start_recalculation);
 126                 }
 127         }
 128 }
 129
 130 static void do_control()
 131 {
 132         double speedl, speedr;
 133         pthread_mutex_lock(&actual_trajectory_lock);
 134         Trajectory *w = actual_trajectory;
 135         if (w) {
 136                 Pos ref_pos, est_pos, balet_out;
 137                 bool done;
 138                 double t;
 139                 struct timeval tv;
 140
 141                 // Calculate reference position
 142                 gettimeofday(&tv, NULL);
 143                 t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
 144                 t += (tv.tv_sec - tv_start.tv_sec);
 145
 146                 if (robot.obstacle_avoidance_enabled)
 147                         check_for_collision_in_future(w, t);
 148
 149                 done = w->getRefPos(t, ref_pos);
 150
 151                 ROBOT_LOCK(ref_pos);
 152                 robot.ref_pos.x = ref_pos.x;
 153                 robot.ref_pos.y = ref_pos.y;
 154                 robot.ref_pos.phi = ref_pos.phi;
 155                 ROBOT_UNLOCK(ref_pos);
 156
 157                 ROBOT_LOCK(est_pos);
 158                 est_pos.x = robot.est_pos.x;
 159                 est_pos.y = robot.est_pos.y;
 160                 est_pos.phi = robot.est_pos.phi;
 161                 ROBOT_UNLOCK(est_pos);
 162
 163 #ifdef CONFIG_OPEN_LOOP
 164                 // We don't use any feedback now. It is
 165                 // supposed that the estimated position is
 166                 // equal to the reference position.
 167                 robot_set_est_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
 168                 est_pos.x = ref_pos.x;
 169                 est_pos.y = ref_pos.y;
 170                 est_pos.phi = ref_pos.phi;
 171 #endif
 172
 173 #ifdef NEVER //DEBUG
 174                 printf("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f  omega=%-4.02f\r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega);
 175                 fflush(stdout);
 176 #endif
 177                 // Call the controller
 178                 double error;
 179                 error = balet(ref_pos, est_pos, k, balet_out);
 180                 speedl = balet_out.v - ROBOT_ROTATION_RADIUS_M*balet_out.omega;
 181                 speedr = balet_out.v + ROBOT_ROTATION_RADIUS_M*balet_out.omega;
 182                 notify_fsm(done, error);
 183         } else {
 184                 speedl = 0;
 185                 speedr = 0;
 186         }
 187
 188
 189         // Apply controller output
 190         robot_send_speed(speedl, speedr);
 191         pthread_mutex_unlock(&actual_trajectory_lock);
 192 }
 193
 194 /**
 195  * A thread running the controller.
 196  *
 197  * This (high priority) thread executes the motion control
 198  * algorithm. It calculates repference position based on actual
 199  * trajectory and current time. Then it calls "balet" controller to
 200  * close feedback.
 201  *
 202  * @param arg
 203  *
 204  * @return
 205  */
 206 static void *thread_trajectory_follower(void *arg)
 207 {
 208         struct itimerspec ts;
 209         sigset_t sigset;
 210         struct sigevent sigevent;
 211         timer_t timer;
 212
 213         ts.it_value.tv_sec = 0;
 214         ts.it_value.tv_nsec = MOTION_PERIOD_NS;
 215         ts.it_interval.tv_sec = 0;
 216         ts.it_interval.tv_nsec = MOTION_PERIOD_NS;
 217
 218         sigemptyset(&sigset);
 219         sigaddset(&sigset, SIG_TIMER);
 220         sigaddset(&sigset, SIG_NEWDATA);
 221
 222         pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*) NULL);
 223
 224         sigevent.sigev_notify = SIGEV_SIGNAL;
 225         sigevent.sigev_signo = SIG_TIMER;
 226         sigevent.sigev_value.sival_int = 0;
 227
 228         if (timer_create(CLOCK_REALTIME, &sigevent, &timer) < 0) {
 229                 perror("move: timer_create");
 230                 return NULL;
 231         }
 232
 233         if (timer_settime(timer, 0, &ts, NULL) < 0) {
 234                 perror("move: timer_settimer");
 235                 return NULL;
 236         }
 237
 238         while (1) {
 239                 int sig;
 240                 // Wait for start of new period or new data
 241                 sigwait(&sigset, &sig);
 242                 //DBG("signal %d ", sig - SIGRTMIN);
 243                 do_control();
 244         }
 245 }
 246
 247 /**
 248  * Tells trajctory_follower to start moving along trajectory @c t.
 249  *
 250  * @param t Trajectory to follow.
 251  */
 252 static void go(Trajectory *t)
 253 {
 254         Trajectory *old;
 255         pthread_mutex_lock(&actual_trajectory_lock);
 256         old = actual_trajectory;
 257         gettimeofday(&tv_start, NULL);
 258         actual_trajectory = t;
 259         pthread_mutex_unlock(&actual_trajectory_lock);
 260         if (old)
 261                 delete(old);
 262 }
 263
 264 static void stop()
 265 {
 266         delete_actual_trajectory();
 267         pthread_kill(thr_trajectory_follower, SIG_NEWDATA);
 268 }
 269
 270 enum target_status {
 271         TARGET_OK,
 272         TARGET_NOT_REACHABLE,
 273         TARGET_ERROR
 274 };
 275 /**
 276  * @brief       Calculates and simplify a path to goal position avoiding obstacles.
 277  * @return      True on succes, false on error.
 278  */
 279 static enum target_status new_goal(struct move_target *target)
 280 {
 281         enum target_status ret;
 282         double startx, starty, angle;
 283         PathPoint * path = NULL;
 284         PathPoint * tmp_point = NULL;
 285
 286         struct TrajectoryConstraints tc;
 287         Pos start;
 288         //tc.maxv *= 2.5;
 289         //tc.maxacc *= 2.5;
 290
 291         /// Get actual position.
 292         ROBOT_LOCK(est_pos);
 293         startx = robot.est_pos.x;
 294         starty = robot.est_pos.y;
 295         start.x = startx;
 296         start.y = starty;
 297         start.phi = robot.est_pos.phi;
 298         ROBOT_UNLOCK(est_pos);
 299         tc = target->tc;
 300
 301         //DBG("Going from (%.2f, %.2f) to (%.2f, %.2f)\n", startx, starty, goalx, goaly);
 302         /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
 303         switch (path_planner(startx, starty, target->x, target->y, &path, &angle)) {
 304                 case PP_ERROR_MAP_NOT_INIT:
 305                         ret = TARGET_ERROR; break;
 306                 case PP_ERROR_GOAL_IS_OBSTACLE:
 307                 case PP_ERROR_GOAL_NOT_REACHABLE:
 308                         ret = TARGET_NOT_REACHABLE; break;
 309                 default:
 310                         ret = TARGET_OK; break;
 311         }
 312
 313         if (ret != TARGET_OK)
 314                 return ret;
 315
 316         bool backward = false;
 317         Trajectory *t = new Trajectory(tc, backward);
 318
 319         // Add this  path to the trajectory.
 320         for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) {
 321                 //DBG("ADDDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y);
 322                 t->addPoint(tmp_point->x, tmp_point->y);
 323         }
 324
 325         t->finalHeading = target->h;
 326         freePathMemory(path);
 327
 328         if (t->prepare(start)) {
 329                 go(t);
 330         } else {
 331                 delete(t);
 332                 ret = TARGET_ERROR;
 333         }
 334         return ret;
 335 }
 336
 337
 338 /**
 339  * Starts moving on trajectory prepared by functions in
 340  * movehelper.cc. In case of error it sends the proper event to MAIN
 341  * FSM.
 342  *
 343  * @return true on succes, false on error.
 344  */
 345 static enum target_status new_trajectory(Trajectory *t)
 346 {
 347         Pos pos;
 348
 349         if (!t) {
 350                 printf("ERROR: No trajectory\n");
 351                 return TARGET_ERROR;
 352         }
 353         ROBOT_LOCK(est_pos);
 354         pos.x = robot.est_pos.x;
 355         pos.y = robot.est_pos.y;
 356         pos.phi = robot.est_pos.phi;
 357         ROBOT_UNLOCK(est_pos);
 358
 359         if (t->prepare(pos)) {
 360                 go(t);
 361                 return TARGET_OK;
 362         } else {
 363                 delete(t);
 364                 return TARGET_ERROR;
 365         }
 366 }
 367
 368 static enum target_status new_target(struct move_target *target)
 369 {
 370         enum target_status ret;
 371         if (target->trajectory) {
 372                 ret = new_trajectory((Trajectory*)target->trajectory);
 373                 // Trajectory is deleted by somebody else
 374                 target->trajectory = NULL;
 375         } else {
 376                 ret = new_goal(target);
 377         }
 378         if (ret != TARGET_ERROR) {
 379                 current_target = *target;
 380         }
 381         return ret;
 382 }
 383
 384 /* This recalculation must only be triggered from mvoement state. */
 385 void *thread_trajectory_realculation(void *arg)
 386 {
 387         while (1) {
 388                 sem_wait(&start_recalculation);
 389                 /* TODO: Different start for planning than esitmated position */
 390                 enum target_status ret = new_goal(&current_target);
 391                 switch (ret) {
 392                         case TARGET_OK:
 393                                 break;
 394                         case TARGET_ERROR:
 395                                 printf("Target error on recalculation_in_progress\n");
 396                                 FSM_SIGNAL(MOTION, EV_RECALCULATION_FAILED, NULL);
 397                                 break;
 398                         case TARGET_NOT_REACHABLE:
 399                                 FSM_SIGNAL(MOTION, EV_RECALCULATION_FAILED, NULL);
 400                                 break;
 401                 }
 402                 sem_post(&recalculation_not_running);
 403         }
 404 }
 405
 406 /*******************************************************************************
 407  * The automaton
 408  *******************************************************************************/
 409
 410 FSM_STATE_DECL(movement);
 411 FSM_STATE_DECL(close_to_target);
 412 FSM_STATE_DECL(wait_for_command);
 413 FSM_STATE_DECL(reset_mcl);
 414 FSM_STATE_DECL(wait_and_try_again);
 415
 416
 417 FSM_STATE(init)
 418 {
 419         pthread_attr_t tattr;
 420         sched_param param;
 421         int ret;
 422
 423         if (FSM_EVENT == EV_ENTRY) {
 424                 actual_trajectory = NULL;
 425
 426                 // Trajectory follower thread
 427                 pthread_attr_init (&tattr);
 428                 pthread_attr_getschedparam (&tattr, &param);
 429                 param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER;
 430                 ret = pthread_attr_setschedparam (&tattr, &param);
 431                 if(ret) {
 432                         perror("move_init: pthread_attr_setschedparam(follower)");
 433                         exit(1);
 434                 }
 435                 ret = pthread_create(&thr_trajectory_follower, &tattr, thread_trajectory_follower, NULL);
 436                 if(ret) {
 437                         perror("move_init: pthread_create");
 438                         exit(1);
 439                 }
 440
 441                 // Trajectory follower thread
 442                 sem_init(&recalculation_not_running, 0, 1);
 443                 sem_init(&start_recalculation, 0, 0);
 444                 pthread_attr_init (&tattr);
 445                 pthread_attr_getschedparam (&tattr, &param);
 446                 param.sched_priority = THREAD_PRIO_TRAJ_RECLAC;
 447                 ret = pthread_attr_setschedparam (&tattr, &param);
 448                 if(ret) {
 449                         perror("move_init: pthread_attr_setschedparam(follower)");
 450                         exit(1);
 451                 }
 452                 ret = pthread_create(&thr_trajctory_recalculation, &tattr, thread_trajectory_realculation, NULL);
 453                 if(ret) {
 454                         perror("move_init: pthread_create");
 455                         exit(1);
 456                 }
 457
 458                 FSM_TRANSITION(wait_for_command);
 459         }
 460 }
 461
 462 #define FIND_NEW_WAY(target)                                    \
 463 do {                                                            \
 464         enum target_status ret = new_goal(target);              \
 465         switch (ret) {                                          \
 466                 case TARGET_OK:                                 \
 467                         FSM_TRANSITION(movement);               \
 468                         break;                                  \
 469                 case TARGET_ERROR:                              \
 470                         printf("Target error\n");               \
 471                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); \
 472                         break;                                  \
 473                 case TARGET_NOT_REACHABLE:                      \
 474                         FSM_TRANSITION(wait_and_try_again);     \
 475                         break;                                  \
 476         }                                                       \
 477 } while(0)
 478
 479
 480 FSM_STATE(wait_for_command)
 481 {
 482         enum target_status ret;
 483         switch (FSM_EVENT) {
 484                 case EV_ENTRY:
 485                         stop();
 486                         break;
 487                 case EV_NEW_TARGET:
 488                         ret = new_target((struct move_target*)FSM_EVENT_PTR);
 489                         switch (ret) {
 490                                 case TARGET_OK:
 491                                         FSM_TRANSITION(movement);
 492                                         break;
 493                                 case TARGET_ERROR:
 494                                         printf("Target error\n");
 495                                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 496                                         break;
 497                                 case TARGET_NOT_REACHABLE:
 498                                         FSM_TRANSITION(wait_and_try_again);
 499                                         break;
 500                         }
 501                         free(FSM_EVENT_PTR);
 502                         break;
 503                 default:
 504                         break;
 505         }
 506 }
 507
 508 FSM_STATE(movement)
 509 {
 510         switch (FSM_EVENT) {
 511                 case EV_TRAJECTORY_DONE:
 512                         FSM_TRANSITION(close_to_target);
 513                         break;
 514                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 515                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 516                         FSM_TRANSITION(wait_for_command);
 517                         break;
 518                 case EV_RECALCULATION_FAILED:
 519                         FSM_TRANSITION(wait_and_try_again);
 520                         break;
 521                 case EV_TRAJECTORY_LOST:
 522                         FSM_TRANSITION(reset_mcl);
 523                         break;
 524                 case EV_MOVE_STOP:
 525                         FSM_TRANSITION(wait_for_command);
 526                         break;
 527                 case EV_ENTRY:
 528                 case EV_EXIT:
 529                         break;
 530                 case EV_NEW_TARGET:
 531                 case EV_TIMER:
 532                 case EV_RETURN:
 533                 case EV_FOUND_AFTER_RESET:
 534                         DBG_PRINT_EVENT("unhandled event");
 535                         break;
 536         }
 537 }
 538
 539 FSM_STATE(close_to_target)
 540 {
 541         switch (FSM_EVENT) {
 542                 case EV_RETURN:
 543                         if (FSM_EVENT_INT == EV_MOVE_STOP)
 544                                 FSM_TRANSITION(wait_for_command);
 545                         //
 546                 case EV_ENTRY:
 547                         FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
 548                         break;
 549                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 550                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 551                         FSM_TRANSITION(wait_for_command);
 552                         break;
 553                 case EV_TRAJECTORY_LOST:
 554                 case EV_TIMER:
 555                         FSM_TRANSITION(reset_mcl);
 556                         break;
 557                 case EV_MOVE_STOP:
 558                         FSM_TRANSITION(wait_for_command);
 559                         break;
 560                 case EV_EXIT:
 561                         stop();
 562                         break;
 563                 case EV_TRAJECTORY_DONE:
 564                 case EV_NEW_TARGET:
 565                 case EV_FOUND_AFTER_RESET:
 566                 case EV_RECALCULATION_FAILED:
 567                         DBG_PRINT_EVENT("unhandled event");
 568                         break;
 569         }
 570 }
 571
 572 FSM_STATE(reset_mcl)
 573 {
 574         switch (FSM_EVENT) {
 575                 case EV_ENTRY:
 576                         stop();
 577                         FSM_SIGNAL(LOC, EV_RESET, NULL);
 578                         break;
 579                 case EV_FOUND_AFTER_RESET:
 580                         FIND_NEW_WAY(&current_target);
 581                         break;
 582                 case EV_MOVE_STOP:
 583                         FSM_TRANSITION(wait_for_command);
 584                         break;
 585                 case EV_EXIT:
 586                         break;
 587                 case EV_TRAJECTORY_DONE:
 588                 case EV_NEW_TARGET:
 589                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 590                 case EV_TRAJECTORY_LOST:
 591                 case EV_RETURN:
 592                 case EV_TIMER:
 593                 case EV_RECALCULATION_FAILED:
 594                         DBG_PRINT_EVENT("unhandled event");
 595         }
 596 }
 597
 598
 599 FSM_STATE(wait_and_try_again)
 600 {
 601         switch (FSM_EVENT) {
 602                 case EV_ENTRY:
 603                         stop(); //FIXME: Not hard stop
 604                         FSM_TIMER(1000);
 605                         break;
 606                 case EV_TIMER:
 607                         FIND_NEW_WAY(&current_target);
 608                         break;
 609                 case EV_MOVE_STOP:
 610                         FSM_TRANSITION(wait_for_command);
 611                         break;
 612                 case EV_TRAJECTORY_DONE:
 613                 case EV_NEW_TARGET:
 614                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 615                 case EV_TRAJECTORY_LOST:
 616                 case EV_RETURN:
 617                 case EV_FOUND_AFTER_RESET:
 618                 case EV_RECALCULATION_FAILED:
 619                         DBG_PRINT_EVENT("unhandled event");
 620                         break;
 621                 case EV_EXIT:
 622                         break;
 623         }
 624 }