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         switch (FSM_EVENT) {
 306                 case EV_ENTRY:
 307                         break;
 308                 case EV_TRAJECTORY_DONE:
 309                         // Skip close to target because sometimes it turn the robot to much
 310 //                      FSM_TRANSITION(close_to_target);
 311 //                      break;
 312                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 313                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 314                         FSM_TRANSITION(wait_for_command);
 315                         break;
 316                 case EV_OBSTACLE:
 317                         stop();
 318                         switch (recalculate_trajectory()) {
 319                                 // We can go to the target:
 320                                 case TARGET_OK:    break;
 321                                 // Target inaccessible because of an obstacle
 322                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 323                                 // Fatal error during planning
 324                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 325                                                    ul_logerr("Target error\n");
 326                                                    /* FIXME (Filip): shouldn't transition to wait_for_command be here?
 327                                                       No, probably not, if an error occurs, robot won't move any more
 328                                                       and we can recognize the error. */
 329                                                    break;
 330                         }
 331                         break;
 332                 case EV_OBSTACLE_BEHIND:
 333                         if (robot.moves_backward && robot.use_rear_bumper)
 334                                 FSM_TRANSITION(wait_and_try_again);
 335                         break;
 336                 case EV_OBSTACLE_SIDE:
 337                         if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
 338                             (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
 339                                 FSM_TRANSITION(wait_and_try_again);
 340                         break;
 341                 case EV_TRAJECTORY_LOST:
 342                         FSM_TRANSITION(lost);
 343                         break;
 344                 case EV_MOVE_STOP:
 345                         FSM_TRANSITION(wait_for_command);
 346                         break;
 347                 case EV_EXIT:
 348                         break;
 349                 case EV_NEW_TARGET:
 350                 case EV_TIMER:
 351                 case EV_RETURN:
 352                         DBG_PRINT_EVENT("unhandled event");
 353                         break;
 354         }
 355 }
 356
 357 FSM_STATE(close_to_target)
 358 {
 359         switch (FSM_EVENT) {
 360                 case EV_ENTRY:
 361                         FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
 362                         break;
 363                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 364                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 365                         FSM_TRANSITION(wait_for_command);
 366                         break;
 367                 case EV_OBSTACLE_BEHIND:
 368                         if (robot.moves_backward && robot.use_rear_bumper)
 369                                 FSM_TRANSITION(wait_and_try_again);
 370                         break;
 371                 case EV_OBSTACLE_SIDE:
 372                         if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
 373                             (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
 374                                 FSM_TRANSITION(wait_and_try_again);
 375                         break;
 376                 case EV_TRAJECTORY_LOST:
 377                 case EV_TIMER:
 378                         FSM_TRANSITION(lost);
 379                         break;
 380                 case EV_MOVE_STOP:
 381                         FSM_TRANSITION(wait_for_command);
 382                         break;
 383                 case EV_EXIT:
 384                         stop();
 385                         break;
 386                 case EV_OBSTACLE:
 387                 case EV_RETURN:
 388                 case EV_TRAJECTORY_DONE:
 389                 case EV_NEW_TARGET:
 390                         DBG_PRINT_EVENT("unhandled event");
 391                         break;
 392         }
 393 }
 394
 395 FSM_STATE(lost)
 396 {
 397         switch (FSM_EVENT) {
 398                 case EV_ENTRY:
 399                         stop();
 400                         FSM_TIMER(1000);
 401                         break;
 402                 case EV_TIMER:
 403                         switch (recalculate_trajectory()) {
 404                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 405                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 406                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
 407                         }
 408                         break;
 409                 case EV_MOVE_STOP:
 410                         FSM_TRANSITION(wait_for_command);
 411                         break;
 412                 case EV_EXIT:
 413                         break;
 414                 case EV_OBSTACLE_BEHIND:
 415                 case EV_OBSTACLE_SIDE:
 416                 case EV_TRAJECTORY_DONE:
 417                 case EV_NEW_TARGET:
 418                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 419                 case EV_TRAJECTORY_LOST:
 420                 case EV_RETURN:
 421                 case EV_OBSTACLE:
 422                         DBG_PRINT_EVENT("unhandled event");
 423         }
 424 }
 425
 426 FSM_STATE(wait_and_try_again)
 427 {
 428         static int target_inacc_cnt = 0;
 429
 430         switch (FSM_EVENT) {
 431                 case EV_ENTRY:
 432                         stop(); //FIXME: Not hard stop
 433                         FSM_TIMER(1000);
 434                         break;
 435                 case EV_TIMER:
 436                         switch (recalculate_trajectory()) {
 437                                 // We can go to the target:
 438                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 439                                 // Target inaccessible because of an obstacle
 440                                 case TARGET_INACC:
 441                                         if (++target_inacc_cnt < 5) {
 442                                                 FSM_TRANSITION(wait_and_try_again);
 443                                                 ul_logerr("Inaccessible\n");
 444                                                 FSM_TIMER(1000);
 445                                                 /* FIXME (Filip): this could happen forever */
 446                                         } else { /* go away if the point is not accessible, try max. 5x */
 447                                                 target_inacc_cnt = 0;
 448                                                 FSM_TRANSITION(wait_for_command);
 449                                                 FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
 450                                                 DBG_PRINT_EVENT("Target inaccessible, go to new target.");
 451                                         }
 452                                         break;
 453                                 // Fatal error during planning
 454                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
 455                         }
 456                         break;
 457                 case EV_TRAJECTORY_DONE:
 458                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 459                         FSM_TRANSITION(wait_for_command);
 460                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 461                         break;
 462                 case EV_MOVE_STOP:
 463                         FSM_TRANSITION(wait_for_command);
 464                         break;
 465                 case EV_OBSTACLE_BEHIND:
 466                 case EV_OBSTACLE_SIDE:
 467                         break;
 468                 case EV_NEW_TARGET:
 469                 case EV_TRAJECTORY_LOST:
 470                 case EV_RETURN:
 471                 case EV_OBSTACLE:
 472                         DBG_PRINT_EVENT("unhandled event");
 473                         break;
 474                 case EV_EXIT:
 475                         break;
 476         }
 477 }