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       Calculates and simplify 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, target->x, target->y, &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 static enum target_status new_target(struct move_target *target)
 154 {
 155         enum target_status ret;
 156         if (target->trajectory) {
 157                 Trajectory *t = (Trajectory*)target->trajectory;
 158                 target->tc = t->constr;
 159                 ret = new_trajectory(t);
 160                 target->trajectory = NULL;
 161                 // Trajectory is deleted by somebody else
 162         } else {
 163                 ret = new_goal(target, 0);
 164         }
 165         if (ret != TARGET_ERROR) {
 166                 current_target = *target;
 167                 // On recaclulation we always use path planner
 168                 current_target.use_planning = true;
 169         }
 170         return ret;
 171 }
 172
 173 enum target_status
 174 recalculate_trajectory(void)
 175 {
 176         /* TODO: Different start for planning than esitmated position */
 177         enum target_status ret = new_goal(&current_target, TRGEN_DELAY);
 178         switch (ret) {
 179         case TARGET_OK:
 180                 break;
 181         case TARGET_ERROR:
 182                 printf("Target error on recalculation_in_progress\n");
 183                 break;
 184         case TARGET_INACC:
 185                 break;
 186         }
 187
 188         int val;
 189         sem_getvalue(&recalculation_not_running, &val);
 190         if (val == 0)
 191                 sem_post(&recalculation_not_running);
 192         return ret;
 193 }
 194
 195 /*******************************************************************************
 196  * The automaton
 197  *******************************************************************************/
 198
 199 FSM_STATE_DECL(movement);
 200 FSM_STATE_DECL(close_to_target);
 201 FSM_STATE_DECL(wait_for_command);
 202 FSM_STATE_DECL(lost);
 203 FSM_STATE_DECL(wait_and_try_again);
 204
 205
 206 FSM_STATE(init)
 207 {
 208         if (FSM_EVENT == EV_ENTRY) {
 209                 FSM_TRANSITION(wait_for_command);
 210         }
 211 }
 212
 213 FSM_STATE(wait_for_command)
 214 {
 215         enum target_status ret;
 216         switch (FSM_EVENT) {
 217                 case EV_ENTRY:
 218                         stop();
 219                         break;
 220                 case EV_NEW_TARGET:
 221                         ret = new_target((struct move_target*)FSM_EVENT_PTR);
 222                         switch (ret) {
 223                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 224                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 225                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 226                         }
 227                         free(FSM_EVENT_PTR);
 228                         break;
 229                 default:
 230                         break;
 231         }
 232 }
 233
 234 FSM_STATE(movement)
 235 {
 236         switch (FSM_EVENT) {
 237                 case EV_TRAJECTORY_DONE:
 238                         // Skip close to target because sometimes it turn the robot to much
 239 //                      FSM_TRANSITION(close_to_target);
 240 //                      break;
 241                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 242                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 243                         FSM_TRANSITION(wait_for_command);
 244                         break;
 245                 case EV_OBSTACLE:
 246                         switch (recalculate_trajectory()) {
 247                                 case TARGET_OK:    break;
 248                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 249                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 250                         }
 251                         break;
 252                 case EV_TRAJECTORY_LOST:
 253                         FSM_TRANSITION(lost);
 254                         break;
 255                 case EV_MOVE_STOP:
 256                         FSM_TRANSITION(wait_for_command);
 257                         break;
 258                 case EV_ENTRY:
 259                 case EV_EXIT:
 260                         break;
 261                 case EV_NEW_TARGET:
 262                 case EV_TIMER:
 263                 case EV_RETURN:
 264                         DBG_PRINT_EVENT("unhandled event");
 265                         break;
 266         }
 267 }
 268
 269 FSM_STATE(close_to_target)
 270 {
 271         switch (FSM_EVENT) {
 272                 case EV_ENTRY:
 273                         FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
 274                         break;
 275                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 276                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 277                         FSM_TRANSITION(wait_for_command);
 278                         break;
 279                 case EV_TRAJECTORY_LOST:
 280                 case EV_TIMER:
 281                         FSM_TRANSITION(lost);
 282                         break;
 283                 case EV_MOVE_STOP:
 284                         FSM_TRANSITION(wait_for_command);
 285                         break;
 286                 case EV_EXIT:
 287                         stop();
 288                         break;
 289                 case EV_OBSTACLE:
 290                 case EV_RETURN:
 291                 case EV_TRAJECTORY_DONE:
 292                 case EV_NEW_TARGET:
 293                         DBG_PRINT_EVENT("unhandled event");
 294                         break;
 295         }
 296 }
 297
 298 FSM_STATE(lost)
 299 {
 300         switch (FSM_EVENT) {
 301                 case EV_ENTRY:
 302                         stop();
 303                         FSM_TIMER(1000);
 304                         break;
 305                 case EV_TIMER:
 306                         switch (recalculate_trajectory()) {
 307                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 308                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 309                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 310                         }
 311                         break;
 312                 case EV_MOVE_STOP:
 313                         FSM_TRANSITION(wait_for_command);
 314                         break;
 315                 case EV_EXIT:
 316                         break;
 317                 case EV_TRAJECTORY_DONE:
 318                 case EV_NEW_TARGET:
 319                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 320                 case EV_TRAJECTORY_LOST:
 321                 case EV_RETURN:
 322                 case EV_OBSTACLE:
 323                         DBG_PRINT_EVENT("unhandled event");
 324         }
 325 }
 326
 327 FSM_STATE(wait_and_try_again)
 328 {
 329         switch (FSM_EVENT) {
 330                 case EV_ENTRY:
 331                         stop(); //FIXME: Not hard stop
 332                         FSM_TIMER(1000);
 333                         break;
 334                 case EV_TIMER:
 335                         switch (recalculate_trajectory()) {
 336                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 337                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 338                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 339                         }
 340                         break;
 341                 case EV_TRAJECTORY_DONE:
 342                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 343                         FSM_TRANSITION(wait_for_command);
 344                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 345                         break;
 346                 case EV_MOVE_STOP:
 347                         FSM_TRANSITION(wait_for_command);
 348                         break;
 349                 case EV_NEW_TARGET:
 350                 case EV_TRAJECTORY_LOST:
 351                 case EV_RETURN:
 352                 case EV_OBSTACLE:
 353                         DBG_PRINT_EVENT("unhandled event");
 354                         break;
 355                 case EV_EXIT:
 356                         break;
 357         }
 358 }