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