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  * @brief       FIXME Calculates and simplifies a path to goal position avoiding obstacles.
  49  * @param start_in_future is delay utilised to calculate the trajectory while robot
  50  * is moving (zero means to start movement from the current position)
  51  * @return      True on succes, false on error.
  52  */
  53 static enum target_status new_goal(struct move_target *target, double start_in_future)
  54 {
  55         enum target_status ret;
  56         double angle, targetx, targety;
  57         PathPoint * path = NULL;
  58         PathPoint * tmp_point = NULL;
  59         Pos start;
  60         double time_ts;
  61         bool backward = false;
  62
  63         // Where to start trajectory planning?
  64         get_future_pos(start_in_future, start, time_ts);
  65         Trajectory *t = new Trajectory(target->tc, backward);
  66
  67         if (target->heading.operation == TOP_ARRIVE_FROM) {
  68                 get_arrive_from_point(target->x, target->y, target->heading,
  69                                       &targetx, &targety);
  70         } else {
  71                 targetx = target->x;
  72                 targety = target->y;
  73         }
  74
  75         if (target->use_planning) {
  76                 /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
  77                 switch (path_planner(start.x, start.y, targetx, targety, &path, &angle)) {
  78                         case PP_ERROR_MAP_NOT_INIT:
  79                                 ret = TARGET_ERROR; break;
  80                         case PP_ERROR_GOAL_IS_OBSTACLE:
  81                         case PP_ERROR_GOAL_NOT_REACHABLE:
  82                                 ret = TARGET_INACC; break;
  83                         default:
  84                                 ret = TARGET_OK; break;
  85                 }
  86
  87                 if (ret != TARGET_OK)
  88                         return ret;
  89                 // Add this  path to the trajectory.
  90                 for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) {
  91                         DBG("ADDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y);
  92                         t->addPoint(tmp_point->x, tmp_point->y);
  93                 }
  94                 freePathMemory(path);
  95         } else {
  96                 t->addPoint(targetx, targety);
  97         }
  98
  99         if (target->heading.operation == TOP_ARRIVE_FROM) {
 100                 t->addPoint(target->x, target->y);
 101         }
 102         t->finalHeading = target2final_heading(target->heading);
 103
 104         if (t->prepare(start)) {
 105                 if (start_in_future) {
 106                         /* not to switch immediately
 107                            switch_trajectory_at(t, time_ts); */
 108                         // new test - appending trajectories
 109                         go(t, time_ts);
 110                 } else {
 111
 112                         go(t, 0);
 113                 }
 114         } else {
 115                 delete(t);
 116                 ret = TARGET_ERROR;
 117         }
 118
 119         return ret;
 120 }
 121
 122
 123 /**
 124  * Starts moving on trajectory prepared by functions in
 125  * movehelper.cc. In case of error it sends the proper event to MAIN
 126  * FSM.
 127  *
 128  * @return TARGET_OK on succes, TARGET_ERROR on error.
 129  */
 130 static enum target_status new_trajectory(Trajectory *t)
 131 {
 132         Pos pos;
 133
 134         if (!t) {
 135                 printf("ERROR: No trajectory\n");
 136                 return TARGET_ERROR;
 137         }
 138         ROBOT_LOCK(est_pos);
 139         pos.x = robot.est_pos.x;
 140         pos.y = robot.est_pos.y;
 141         pos.phi = robot.est_pos.phi;
 142         ROBOT_UNLOCK(est_pos);
 143
 144         if (t->prepare(pos)) {
 145                 go(t, 0);
 146                 return TARGET_OK;
 147         } else {
 148                 delete(t);
 149                 return TARGET_ERROR;
 150         }
 151 }
 152
 153 /**
 154  * Handles new target signal from main FSM
 155  *
 156  * @param target
 157  *
 158  * @return
 159  */
 160 static enum target_status new_target(struct move_target *target)
 161 {
 162         enum target_status ret;
 163         if (target->trajectory) {
 164                 Trajectory *t = (Trajectory*)target->trajectory;
 165                 target->tc = t->constr;
 166                 ret = new_trajectory(t);
 167                 target->trajectory = NULL;
 168                 // Trajectory is deleted by somebody else
 169         } else {
 170                 ret = new_goal(target, 0);
 171         }
 172         if (ret != TARGET_ERROR) {
 173                 current_target = *target;
 174         }
 175         return ret;
 176 }
 177
 178 /**
 179  * Recalculates trajectory to previous move target specified by
 180  * new_target()
 181  *
 182  * @return
 183  */
 184 enum target_status
 185 recalculate_trajectory(void)
 186 {
 187         /* TODO: Different start for planning than esitmated position */
 188         enum target_status ret;
 189         current_target.use_planning = true;
 190         ret = new_goal(&current_target, TRGEN_DELAY);
 191         switch (ret) {
 192         case TARGET_OK:
 193                 break;
 194         case TARGET_ERROR:
 195                 printf("Target error on recalculation_in_progress\n");
 196                 break;
 197         case TARGET_INACC:
 198                 break;
 199         }
 200
 201         int val;
 202         sem_getvalue(&recalculation_not_running, &val);
 203         if (val == 0)
 204                 sem_post(&recalculation_not_running);
 205         return ret;
 206 }
 207
 208 /*******************************************************************************
 209  * The automaton
 210  *******************************************************************************/
 211
 212 FSM_STATE_DECL(movement);
 213 FSM_STATE_DECL(close_to_target);
 214 FSM_STATE_DECL(wait_for_command);
 215 FSM_STATE_DECL(lost);
 216 FSM_STATE_DECL(wait_and_try_again);
 217
 218
 219 FSM_STATE(init)
 220 {
 221         if (FSM_EVENT == EV_ENTRY) {
 222                 FSM_TRANSITION(wait_for_command);
 223         }
 224 }
 225
 226 FSM_STATE(wait_for_command)
 227 {
 228         enum target_status ret;
 229         switch (FSM_EVENT) {
 230                 case EV_ENTRY:
 231                         stop();
 232                         break;
 233                 case EV_NEW_TARGET:
 234                         ret = new_target((struct move_target*)FSM_EVENT_PTR);
 235                         switch (ret) {
 236                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 237                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 238                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 239                         }
 240                         free(FSM_EVENT_PTR);
 241                         break;
 242                 default:
 243                         break;
 244         }
 245 }
 246
 247 FSM_STATE(movement)
 248 {
 249         switch (FSM_EVENT) {
 250                 case EV_TRAJECTORY_DONE:
 251                         // Skip close to target because sometimes it turn the robot to much
 252 //                      FSM_TRANSITION(close_to_target);
 253 //                      break;
 254                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 255                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 256                         FSM_TRANSITION(wait_for_command);
 257                         break;
 258                 case EV_OBSTACLE:
 259                         switch (recalculate_trajectory()) {
 260                                 case TARGET_OK:    break;
 261                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 262                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 263                         }
 264                         break;
 265                 case EV_OBSTACLE_BEHIND:
 266                         FSM_TRANSITION(wait_and_try_again);
 267                         break;
 268                 case EV_TRAJECTORY_LOST:
 269                         FSM_TRANSITION(lost);
 270                         break;
 271                 case EV_MOVE_STOP:
 272                         FSM_TRANSITION(wait_for_command);
 273                         break;
 274                 case EV_ENTRY:
 275                 case EV_EXIT:
 276                         break;
 277                 case EV_NEW_TARGET:
 278                 case EV_TIMER:
 279                 case EV_RETURN:
 280                         DBG_PRINT_EVENT("unhandled event");
 281                         break;
 282         }
 283 }
 284
 285 FSM_STATE(close_to_target)
 286 {
 287         switch (FSM_EVENT) {
 288                 case EV_ENTRY:
 289                         FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
 290                         break;
 291                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 292                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 293                         FSM_TRANSITION(wait_for_command);
 294                         break;
 295                 case EV_OBSTACLE_BEHIND:
 296                         FSM_TRANSITION(wait_and_try_again);
 297                         break;
 298                 case EV_TRAJECTORY_LOST:
 299                 case EV_TIMER:
 300                         FSM_TRANSITION(lost);
 301                         break;
 302                 case EV_MOVE_STOP:
 303                         FSM_TRANSITION(wait_for_command);
 304                         break;
 305                 case EV_EXIT:
 306                         stop();
 307                         break;
 308                 case EV_OBSTACLE:
 309                 case EV_RETURN:
 310                 case EV_TRAJECTORY_DONE:
 311                 case EV_NEW_TARGET:
 312                         DBG_PRINT_EVENT("unhandled event");
 313                         break;
 314         }
 315 }
 316
 317 FSM_STATE(lost)
 318 {
 319         switch (FSM_EVENT) {
 320                 case EV_ENTRY:
 321                         stop();
 322                         FSM_TIMER(1000);
 323                         break;
 324                 case EV_TIMER:
 325                         switch (recalculate_trajectory()) {
 326                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 327                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 328                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 329                         }
 330                         break;
 331                 case EV_MOVE_STOP:
 332                         FSM_TRANSITION(wait_for_command);
 333                         break;
 334                 case EV_EXIT:
 335                         break;
 336                 case EV_OBSTACLE_BEHIND:
 337                 case EV_TRAJECTORY_DONE:
 338                 case EV_NEW_TARGET:
 339                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 340                 case EV_TRAJECTORY_LOST:
 341                 case EV_RETURN:
 342                 case EV_OBSTACLE:
 343                         DBG_PRINT_EVENT("unhandled event");
 344         }
 345 }
 346
 347 FSM_STATE(wait_and_try_again)
 348 {
 349         switch (FSM_EVENT) {
 350                 case EV_ENTRY:
 351                         stop(); //FIXME: Not hard stop
 352                         FSM_TIMER(1000);
 353                         break;
 354                 case EV_TIMER:
 355                         switch (recalculate_trajectory()) {
 356                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 357                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 358                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 359                         }
 360                         break;
 361                 case EV_TRAJECTORY_DONE:
 362                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 363                         FSM_TRANSITION(wait_for_command);
 364                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 365                         break;
 366                 case EV_MOVE_STOP:
 367                         FSM_TRANSITION(wait_for_command);
 368                         break;
 369                 case EV_OBSTACLE_BEHIND:
 370                         break;
 371                 case EV_NEW_TARGET:
 372                 case EV_TRAJECTORY_LOST:
 373                 case EV_RETURN:
 374                 case EV_OBSTACLE:
 375                         DBG_PRINT_EVENT("unhandled event");
 376                         break;
 377                 case EV_EXIT:
 378                         break;
 379         }
 380 }