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