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_back_bumpers &&
 341                                 (robot.orte.robot_bumpers.bumper_left_across ||
 342                                 robot.orte.robot_bumpers.bumper_right_across ||
 343                                 robot.orte.robot_bumpers.bumper_rear_left ||
 344                                 robot.orte.robot_bumpers.bumper_rear_right))
 345                                 FSM_TRANSITION(wait_and_try_again);
 346                         break;
 347                 case EV_OBSTACLE_SIDE:
 348                         if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
 349                             (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
 350                                 FSM_TRANSITION(wait_and_try_again);
 351                         break;
 352                 case EV_TRAJECTORY_LOST:
 353                         FSM_TRANSITION(lost);
 354                         break;
 355                 case EV_MOVE_STOP:
 356                         FSM_TRANSITION(wait_for_command);
 357                         break;
 358                 case EV_EXIT:
 359                         break;
 360                 case EV_NEW_TARGET:
 361                 case EV_TIMER:
 362                 case EV_RETURN:
 363                         DBG_PRINT_EVENT("unhandled event");
 364                         break;
 365         }
 366 }
 367
 368 FSM_STATE(close_to_target)
 369 {
 370         switch (FSM_EVENT) {
 371                 case EV_ENTRY:
 372                         FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
 373                         break;
 374                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 375                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 376                         FSM_TRANSITION(wait_for_command);
 377                         break;
 378                 case EV_OBSTACLE_BEHIND:
 379                         if (robot.moves_backward && robot.use_back_bumpers &&
 380                                 (robot.orte.robot_bumpers.bumper_left_across ||
 381                                 robot.orte.robot_bumpers.bumper_right_across ||
 382                                 robot.orte.robot_bumpers.bumper_rear_left ||
 383                                 robot.orte.robot_bumpers.bumper_rear_right))
 384                                 FSM_TRANSITION(wait_and_try_again);
 385                         break;
 386                 case EV_OBSTACLE_SIDE:
 387                         if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
 388                             (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
 389                                 FSM_TRANSITION(wait_and_try_again);
 390                         break;
 391                 case EV_TRAJECTORY_LOST:
 392                 case EV_TIMER:
 393                         FSM_TRANSITION(lost);
 394                         break;
 395                 case EV_MOVE_STOP:
 396                         FSM_TRANSITION(wait_for_command);
 397                         break;
 398                 case EV_EXIT:
 399                         stop();
 400                         break;
 401                 case EV_OBSTACLE:
 402                 case EV_RETURN:
 403                 case EV_TRAJECTORY_DONE:
 404                 case EV_NEW_TARGET:
 405                         DBG_PRINT_EVENT("unhandled event");
 406                         break;
 407         }
 408 }
 409
 410 FSM_STATE(lost)
 411 {
 412         switch (FSM_EVENT) {
 413                 case EV_ENTRY:
 414                         stop();
 415                         FSM_TIMER(1000);
 416                         break;
 417                 case EV_TIMER:
 418                         switch (recalculate_trajectory()) {
 419                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 420                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 421                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
 422                         }
 423                         break;
 424                 case EV_MOVE_STOP:
 425                         FSM_TRANSITION(wait_for_command);
 426                         break;
 427                 case EV_EXIT:
 428                         break;
 429                 case EV_OBSTACLE_BEHIND:
 430                 case EV_OBSTACLE_SIDE:
 431                 case EV_TRAJECTORY_DONE:
 432                 case EV_NEW_TARGET:
 433                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 434                 case EV_TRAJECTORY_LOST:
 435                 case EV_RETURN:
 436                 case EV_OBSTACLE:
 437                         DBG_PRINT_EVENT("unhandled event");
 438         }
 439 }
 440
 441 FSM_STATE(wait_and_try_again)
 442 {
 443         static int target_inacc_cnt = 0;
 444
 445         switch (FSM_EVENT) {
 446                 case EV_ENTRY:
 447                         stop(); //FIXME: Not hard stop
 448                         FSM_TIMER(1000);
 449                         break;
 450                 case EV_TIMER:
 451                         switch (recalculate_trajectory()) {
 452                                 // We can go to the target:
 453                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 454                                 // Target inaccessible because of an obstacle
 455                                 case TARGET_INACC:
 456                                         if (++target_inacc_cnt < 3) {
 457                                                 FSM_TRANSITION(wait_and_try_again);
 458                                                 ul_logerr("Inaccessible\n");
 459                                                 FSM_TIMER(1000);
 460                                                 /* FIXME (Filip): this could happen forever */
 461                                         } else { /* go away if the point is not accessible, try max. 3x */
 462                                                 target_inacc_cnt = 0;
 463                                                 FSM_TRANSITION(wait_for_command);
 464                                                 FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
 465                                                 DBG_PRINT_EVENT("Target inaccessible, go to new target.");
 466                                         }
 467                                         break;
 468                                 // Fatal error during planning
 469                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
 470                         }
 471                         break;
 472                 case EV_TRAJECTORY_DONE:
 473                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 474                         FSM_TRANSITION(wait_for_command);
 475                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 476                         break;
 477                 case EV_MOVE_STOP:
 478                         FSM_TRANSITION(wait_for_command);
 479                         break;
 480                 case EV_OBSTACLE_BEHIND:
 481                 case EV_OBSTACLE_SIDE:
 482                         break;
 483                 case EV_NEW_TARGET:
 484                 case EV_TRAJECTORY_LOST:
 485                 case EV_RETURN:
 486                 case EV_OBSTACLE:
 487                         DBG_PRINT_EVENT("unhandled event");
 488                         break;
 489                 case EV_EXIT:
 490                         break;
 491         }
 492 }