src/robofsm/fsmmove.cc

   1 /**
   2  * @file        fsmmove.cc
   3  * @brief       FSM move
   4  * @author      Michal Sojka, Jose Maria Martin Laguna, Petr Beneš
   5  *
   6  */
   7 #define NEVER
   8 #define DEBUG
   9 #define FSM_MOTION
  10 #include <sys/time.h>
  11 #include <time.h>
  12 #include "trgen.h"
  13 #include "balet.h"
  14 #include "robodata.h"
  15 #include <robot.h>
  16 #include <pthread.h>
  17 #include <path_planner.h>
  18 #include <signal.h>
  19 #include <movehelper.h>
  20 #include <sharp.h>
  21 #include <unistd.h>
  22 #include <map.h>
  23 #include "robot_config.h"
  24 #include <robomath.h>
  25 #include "motion-control.h"
  26 #include <hokuyo.h>
  27 #include <ul_log.h>
  28
  29 UL_LOG_CUST(ulogd_fsmmove); /* Log domain name = ulogd + name of the file */
  30
  31 #ifdef MOTION_DEBUG
  32    #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
  33 #else
  34    #define DBG(format, ...)
  35 #endif
  36
  37 #define MAX_WAIT_FOR_CLOSE_MS 2000
  38
  39 // time to calculate new trajectory [seconds]
  40 #define TRGEN_DELAY 0.3
  41
  42
  43 // Target position of the current movement
  44 static struct move_target current_target;
  45
  46 enum target_status {
  47         TARGET_OK,    // We can go the the target
  48         TARGET_INACC, // Target is inaccessible because of an obstacle
  49         TARGET_ERROR  // Fatal errror during planning
  50 };
  51
  52 static double free_space_in_front_of_robot()
  53 {
  54         int i, i1, i2;
  55         int min = 4000, m;
  56         i1 = HOKUYO_DEG_TO_INDEX(-45);
  57         i2 = HOKUYO_DEG_TO_INDEX(+45);
  58         for (i=MIN(i1, i2); i < MAX(i1, i2); i++) {
  59                 m = robot.orte.hokuyo_scan.data[i];
  60                 if (m > 19 && m < min)
  61                         min = m;
  62         }
  63         return min/1000.0 - (ROBOT_AXIS_TO_FRONT_M - HOKUYO_CENTER_OFFSET_M);
  64 }
  65
  66 static bool obstackle_in_front_if_turn(Trajectory *t)
  67 {
  68         double f;
  69         bool ret;
  70
  71         f = free_space_in_front_of_robot();
  72
  73         if (fabs(t->initialTurnBy) < DEG2RAD(95) &&
  74             f > 1.2*(ROBOT_WIDTH_M/2.0)) {
  75                 ret = false;
  76         } else if (f > 1.5*ROBOT_AXIS_TO_BACK_M) {
  77                 ret = false;
  78         } else
  79                 ret = true;
  80
  81         return ret;
  82 }
  83
  84 /**
  85  * @brief       FIXME Calculates and simplifies a path to goal position avoiding obstacles.
  86  * @param start_in_future is delay utilised to calculate the trajectory while robot
  87  * is moving (zero means to start movement from the current position)
  88  * @return      True on succes, false on error.
  89  */
  90 static enum target_status new_goal(struct move_target *move_target, double start_in_future)
  91 {
  92         enum target_status ret;
  93         double angle;
  94         PathPoint * path = NULL;
  95         PathPoint * tmp_point = NULL;
  96         Pos future_traj_point;
  97         Point target(move_target->x, move_target->y);
  98         Point center(PLAYGROUND_WIDTH_M/2.0, PLAYGROUND_HEIGHT_M/2.0);
  99         double time_ts;
 100         bool backward = false;
 101
 102         // Where to start trajectory planning?
 103         get_future_pos(start_in_future, future_traj_point, time_ts);
 104         Point start(future_traj_point.x, future_traj_point.y);
 105
 106         Trajectory *t = new Trajectory(move_target->tc, backward);
 107
 108         if (move_target->heading.operation == TOP_ARRIVE_FROM) {
 109                 get_arrive_from_point(move_target->x, move_target->y, move_target->heading,
 110                                       &target.x, &target.y);
 111         }
 112
 113         if (move_target->use_planning) {
 114                 /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
 115                 switch (path_planner(start.x, start.y, target.x, target.y, &path, &angle)) {
 116                         case PP_ERROR_MAP_NOT_INIT:
 117                                 ret = TARGET_ERROR; break;
 118                         case PP_ERROR_GOAL_IS_OBSTACLE:
 119                         case PP_ERROR_GOAL_NOT_REACHABLE:
 120                                 ret = TARGET_INACC; break;
 121                         default:
 122                                 ret = TARGET_OK; break;
 123                 }
 124
 125                 if (ret != TARGET_OK)
 126                         return ret;
 127                 // Add this  path to the trajectory.
 128                 for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) {
 129                         DBG("ADDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y);
 130                         t->addPoint(tmp_point->x, tmp_point->y);
 131                 }
 132                 freePathMemory(path);
 133         } else {
 134                 t->addPoint(target.x, target.y);
 135         }
 136
 137         if (move_target->heading.operation == TOP_ARRIVE_FROM) {
 138                 t->addPoint(move_target->x, move_target->y);
 139         }
 140         t->finalHeading = target2final_heading(move_target->heading);
 141
 142         if (t->prepare(future_traj_point)) {
 143                 if (obstackle_in_front_if_turn(t))
 144                         ret = TARGET_INACC;
 145                 else {
 146                         if (start_in_future) {
 147                                 /* not to switch immediately
 148                                    switch_trajectory_at(t, time_ts); */
 149                                 // new test - appending trajectories
 150                                 go(t, time_ts);
 151                         } else {
 152                                 go(t, 0);
 153                         }
 154                 }
 155         } else {
 156                 delete(t);
 157                 ret = TARGET_ERROR;
 158         }
 159
 160         return ret;
 161 }
 162
 163
 164 /**
 165  * Starts moving on trajectory prepared by functions in
 166  * movehelper.cc. In case of error it sends the proper event to MAIN
 167  * FSM.
 168  *
 169  * @return TARGET_OK on succes, TARGET_ERROR on error.
 170  */
 171 static enum target_status new_trajectory(Trajectory *t)
 172 {
 173         Pos pos;
 174
 175         if (!t) {
 176                 ul_logerr("ERROR: No trajectory\n");
 177                 return TARGET_ERROR;
 178         }
 179         // FIXME: This is duplicite code with new_goal. Remove this
 180         // function and use always new_goal.
 181         robot_get_est_pos(&pos.x, &pos.y, &pos.phi);
 182         pos.v = 0;
 183         pos.omega = 0;
 184
 185         if (t->prepare(pos)) {
 186                 if (robot.check_turn_safety && obstackle_in_front_if_turn(t)) {
 187                         return TARGET_INACC;
 188                 }
 189                 else
 190                         go(t, 0);
 191                 return TARGET_OK;
 192         } else {
 193                 delete(t);
 194                 return TARGET_ERROR;
 195         }
 196 }
 197
 198 /**
 199  * Handles new target signal from main FSM
 200  *
 201  * @param target
 202  *
 203  * @return
 204  */
 205 static enum target_status new_target(struct move_target *target)
 206 {
 207         enum target_status ret;
 208         if (target->trajectory) {
 209                 Trajectory *t = (Trajectory*)target->trajectory;
 210                 target->tc = t->constr;
 211                 ret = new_trajectory(t);
 212                 target->trajectory = NULL;
 213                 // Trajectory is deleted by somebody else
 214         } else {
 215                 ret = new_goal(target, 0);
 216         }
 217         if (ret != TARGET_ERROR) {
 218                 current_target = *target;
 219         }
 220         return ret;
 221 }
 222
 223 /**
 224  * Recalculates trajectory to previous move target specified by
 225  * new_target()
 226  *
 227  * @return
 228  */
 229 enum target_status
 230 recalculate_trajectory(void)
 231 {
 232         /* TODO: Different start for planning than esitmated position */
 233         enum target_status ret;
 234         current_target.use_planning = true;
 235         ret = new_goal(&current_target, TRGEN_DELAY);
 236         switch (ret) {
 237         case TARGET_OK:
 238                 break;
 239         case TARGET_ERROR:
 240                 ul_logerr("Target error on recalculation_in_progress\n");
 241                 break;
 242         case TARGET_INACC:
 243                 break;
 244         }
 245
 246         int val;
 247         sem_getvalue(&recalculation_not_running, &val);
 248         if (val == 0)
 249                 sem_post(&recalculation_not_running);
 250         return ret;
 251 }
 252
 253 /*******************************************************************************
 254  * The automaton
 255  *******************************************************************************/
 256
 257 FSM_STATE_DECL(movement);
 258 FSM_STATE_DECL(close_to_target);
 259 FSM_STATE_DECL(wait_for_command);
 260 FSM_STATE_DECL(lost);
 261 FSM_STATE_DECL(wait_and_try_again);
 262
 263
 264 FSM_STATE(init)
 265 {
 266         if (FSM_EVENT == EV_ENTRY) {
 267                 FSM_TRANSITION(wait_for_command);
 268         }
 269 }
 270
 271 FSM_STATE(wait_for_command)
 272 {
 273         enum target_status ret;
 274         switch (FSM_EVENT) {
 275                 case EV_ENTRY:
 276                         stop();
 277                         break;
 278                 case EV_NEW_TARGET:
 279                         ret = new_target((struct move_target*)FSM_EVENT_PTR);
 280                         switch (ret) {
 281                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 282                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 283                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
 284                         }
 285                         free(FSM_EVENT_PTR);
 286                         break;
 287                 case EV_TIMER:
 288                         break;
 289                 case EV_TRAJECTORY_DONE:
 290                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 291                 case EV_TRAJECTORY_LOST:
 292                 case EV_MOVE_STOP:
 293                 case EV_OBSTACLE:
 294                 case EV_OBSTACLE_BEHIND:
 295                 case EV_OBSTACLE_SIDE:
 296                 case EV_RETURN:
 297                         DBG_PRINT_EVENT("unhandled event");
 298                 case EV_EXIT:
 299                         break;
 300         }
 301 }
 302
 303 FSM_STATE(movement)
 304 {
 305         static int obstacle_cntr = 0;
 306
 307         switch (FSM_EVENT) {
 308                 case EV_ENTRY:
 309                         break;
 310                 case EV_TRAJECTORY_DONE:
 311                         // Skip close to target because sometimes it turn the robot to much
 312 //                      FSM_TRANSITION(close_to_target);
 313 //                      break;
 314                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 315                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 316                         FSM_TRANSITION(wait_for_command);
 317                         break;
 318                 case EV_OBSTACLE:
 319                         stop();
 320                         if (obstacle_cntr++ > 5) {
 321                                 FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
 322                                 FSM_TRANSITION(wait_for_command);
 323                                 break;
 324                         }
 325                         switch (recalculate_trajectory()) {
 326                                 // We can go to the target:
 327                                 case TARGET_OK:    break;
 328                                 // Target inaccessible because of an obstacle
 329                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 330                                 // Fatal error during planning
 331                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 332                                                    ul_logerr("Target error\n");
 333                                                    /* FIXME (Filip): shouldn't transition to wait_for_command be here?
 334                                                       No, probably not, if an error occurs, robot won't move any more
 335                                                       and we can recognize the error. */
 336                                                    break;
 337                         }
 338                         break;
 339                 case EV_OBSTACLE_BEHIND:
 340                         if (robot.moves_backward && robot.use_rear_bumper)
 341                                 FSM_TRANSITION(wait_and_try_again);
 342                         break;
 343                 case EV_OBSTACLE_SIDE:
 344                         if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
 345                             (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
 346                                 FSM_TRANSITION(wait_and_try_again);
 347                         break;
 348                 case EV_TRAJECTORY_LOST:
 349                         FSM_TRANSITION(lost);
 350                         break;
 351                 case EV_MOVE_STOP:
 352                         FSM_TRANSITION(wait_for_command);
 353                         break;
 354                 case EV_EXIT:
 355                         break;
 356                 case EV_NEW_TARGET:
 357                 case EV_TIMER:
 358                 case EV_RETURN:
 359                         DBG_PRINT_EVENT("unhandled event");
 360                         break;
 361         }
 362 }
 363
 364 FSM_STATE(close_to_target)
 365 {
 366         switch (FSM_EVENT) {
 367                 case EV_ENTRY:
 368                         FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
 369                         break;
 370                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 371                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 372                         FSM_TRANSITION(wait_for_command);
 373                         break;
 374                 case EV_OBSTACLE_BEHIND:
 375                         if (robot.moves_backward && robot.use_rear_bumper)
 376                                 FSM_TRANSITION(wait_and_try_again);
 377                         break;
 378                 case EV_OBSTACLE_SIDE:
 379                         if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
 380                             (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
 381                                 FSM_TRANSITION(wait_and_try_again);
 382                         break;
 383                 case EV_TRAJECTORY_LOST:
 384                 case EV_TIMER:
 385                         FSM_TRANSITION(lost);
 386                         break;
 387                 case EV_MOVE_STOP:
 388                         FSM_TRANSITION(wait_for_command);
 389                         break;
 390                 case EV_EXIT:
 391                         stop();
 392                         break;
 393                 case EV_OBSTACLE:
 394                 case EV_RETURN:
 395                 case EV_TRAJECTORY_DONE:
 396                 case EV_NEW_TARGET:
 397                         DBG_PRINT_EVENT("unhandled event");
 398                         break;
 399         }
 400 }
 401
 402 FSM_STATE(lost)
 403 {
 404         switch (FSM_EVENT) {
 405                 case EV_ENTRY:
 406                         stop();
 407                         FSM_TIMER(1000);
 408                         break;
 409                 case EV_TIMER:
 410                         switch (recalculate_trajectory()) {
 411                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 412                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 413                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
 414                         }
 415                         break;
 416                 case EV_MOVE_STOP:
 417                         FSM_TRANSITION(wait_for_command);
 418                         break;
 419                 case EV_EXIT:
 420                         break;
 421                 case EV_OBSTACLE_BEHIND:
 422                 case EV_OBSTACLE_SIDE:
 423                 case EV_TRAJECTORY_DONE:
 424                 case EV_NEW_TARGET:
 425                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 426                 case EV_TRAJECTORY_LOST:
 427                 case EV_RETURN:
 428                 case EV_OBSTACLE:
 429                         DBG_PRINT_EVENT("unhandled event");
 430         }
 431 }
 432
 433 FSM_STATE(wait_and_try_again)
 434 {
 435         static int target_inacc_cnt = 0;
 436
 437         switch (FSM_EVENT) {
 438                 case EV_ENTRY:
 439                         stop(); //FIXME: Not hard stop
 440                         FSM_TIMER(1000);
 441                         break;
 442                 case EV_TIMER:
 443                         switch (recalculate_trajectory()) {
 444                                 // We can go to the target:
 445                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 446                                 // Target inaccessible because of an obstacle
 447                                 case TARGET_INACC:
 448                                         if (++target_inacc_cnt < 3) {
 449                                                 FSM_TRANSITION(wait_and_try_again);
 450                                                 ul_logerr("Inaccessible\n");
 451                                                 FSM_TIMER(1000);
 452                                                 /* FIXME (Filip): this could happen forever */
 453                                         } else { /* go away if the point is not accessible, try max. 3x */
 454                                                 target_inacc_cnt = 0;
 455                                                 FSM_TRANSITION(wait_for_command);
 456                                                 FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
 457                                                 DBG_PRINT_EVENT("Target inaccessible, go to new target.");
 458                                         }
 459                                         break;
 460                                 // Fatal error during planning
 461                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
 462                         }
 463                         break;
 464                 case EV_TRAJECTORY_DONE:
 465                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 466                         FSM_TRANSITION(wait_for_command);
 467                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 468                         break;
 469                 case EV_MOVE_STOP:
 470                         FSM_TRANSITION(wait_for_command);
 471                         break;
 472                 case EV_OBSTACLE_BEHIND:
 473                 case EV_OBSTACLE_SIDE:
 474                         break;
 475                 case EV_NEW_TARGET:
 476                 case EV_TRAJECTORY_LOST:
 477                 case EV_RETURN:
 478                 case EV_OBSTACLE:
 479                         DBG_PRINT_EVENT("unhandled event");
 480                         break;
 481                 case EV_EXIT:
 482                         break;
 483         }
 484 }