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 void invalidateAcropole(Point point)
  49 {
  50         Point center(PLAYGROUND_WIDTH_M/2.0, PLAYGROUND_HEIGHT_M/2.0);
  51
  52         double a = center.angleTo(point);
  53         for (int i=0; i<5; i++) {
  54                 float x, y, d;
  55                 const float s = 0.05;
  56                 d = 0.1 * (0.5+i);
  57                 x = point.x + d*cos(a);
  58                 y = point.y + d*sin(a);
  59                 ShmapSetRectangleFlag(x-s, y-s, x+s, y+s, MAP_FLAG_INVALIDATE_WALL, 0);
  60         }
  61 }
  62
  63 void invalidateDown(Point point)
  64 {
  65         printf("XXXXXXXXXXXXXXXinvalidateDown %g, %g\n", point.x, point.y);
  66         float x, s;
  67         s=0.1;
  68         x = point.x;
  69         ShmapSetRectangleFlag(x, 0.1, x, 0.4, MAP_FLAG_INVALIDATE_WALL, 0);
  70 }
  71
  72 /**
  73  * @brief       FIXME Calculates and simplifies a path to goal position avoiding obstacles.
  74  * @param start_in_future is delay utilised to calculate the trajectory while robot
  75  * is moving (zero means to start movement from the current position)
  76  * @return      True on succes, false on error.
  77  */
  78 static enum target_status new_goal(struct move_target *move_target, double start_in_future)
  79 {
  80         enum target_status ret;
  81         double angle;
  82         PathPoint * path = NULL;
  83         PathPoint * tmp_point = NULL;
  84         Pos future_traj_point;
  85         Point target(move_target->x, move_target->y);
  86         Point center(PLAYGROUND_WIDTH_M/2.0, PLAYGROUND_HEIGHT_M/2.0);
  87         double time_ts;
  88         bool backward = false;
  89
  90         // Where to start trajectory planning?
  91         get_future_pos(start_in_future, future_traj_point, time_ts);
  92         Point start(future_traj_point.x, future_traj_point.y);
  93
  94         Trajectory *t = new Trajectory(move_target->tc, backward);
  95
  96         // Clear all invalidate flags;
  97         ShmapSetRectangleFlag(0, 0, PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M, 0, MAP_FLAG_INVALIDATE_WALL);
  98         // Set invalidate flags if we are going to walled area
  99         if (target.distanceTo(center) < 0.40)
 100                 invalidateAcropole(target);
 101         if (start.distanceTo(center) < 0.40)
 102                 invalidateAcropole(start);
 103         if (target.y < 0.40)
 104                 invalidateDown(target);
 105         if (start.y < 0.40)
 106                 invalidateDown(start);
 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 (start_in_future) {
 144                         /* not to switch immediately
 145                            switch_trajectory_at(t, time_ts); */
 146                         // new test - appending trajectories
 147                         go(t, time_ts);
 148                 } else {
 149
 150                         go(t, 0);
 151                 }
 152         } else {
 153                 delete(t);
 154                 ret = TARGET_ERROR;
 155         }
 156
 157         return ret;
 158 }
 159
 160
 161 /**
 162  * Starts moving on trajectory prepared by functions in
 163  * movehelper.cc. In case of error it sends the proper event to MAIN
 164  * FSM.
 165  *
 166  * @return TARGET_OK on succes, TARGET_ERROR on error.
 167  */
 168 static enum target_status new_trajectory(Trajectory *t)
 169 {
 170         Pos pos;
 171
 172         if (!t) {
 173                 printf("ERROR: No trajectory\n");
 174                 return TARGET_ERROR;
 175         }
 176         // FIXME: This is duplicite code with new_goal. Remove this
 177         // function and use always new_goal.
 178         robot_get_est_pos(&pos.x, &pos.y, &pos.phi);
 179         pos.v = 0;
 180         pos.omega = 0;
 181
 182         if (t->prepare(pos)) {
 183                 go(t, 0);
 184                 return TARGET_OK;
 185         } else {
 186                 delete(t);
 187                 return TARGET_ERROR;
 188         }
 189 }
 190
 191 /**
 192  * Handles new target signal from main FSM
 193  *
 194  * @param target
 195  *
 196  * @return
 197  */
 198 static enum target_status new_target(struct move_target *target)
 199 {
 200         enum target_status ret;
 201         if (target->trajectory) {
 202                 Trajectory *t = (Trajectory*)target->trajectory;
 203                 target->tc = t->constr;
 204                 ret = new_trajectory(t);
 205                 target->trajectory = NULL;
 206                 // Trajectory is deleted by somebody else
 207         } else {
 208                 ret = new_goal(target, 0);
 209         }
 210         if (ret != TARGET_ERROR) {
 211                 current_target = *target;
 212         }
 213         return ret;
 214 }
 215
 216 /**
 217  * Recalculates trajectory to previous move target specified by
 218  * new_target()
 219  *
 220  * @return
 221  */
 222 enum target_status
 223 recalculate_trajectory(void)
 224 {
 225         /* TODO: Different start for planning than esitmated position */
 226         enum target_status ret;
 227         current_target.use_planning = true;
 228         ret = new_goal(&current_target, TRGEN_DELAY);
 229         switch (ret) {
 230         case TARGET_OK:
 231                 break;
 232         case TARGET_ERROR:
 233                 printf("Target error on recalculation_in_progress\n");
 234                 break;
 235         case TARGET_INACC:
 236                 break;
 237         }
 238
 239         int val;
 240         sem_getvalue(&recalculation_not_running, &val);
 241         if (val == 0)
 242                 sem_post(&recalculation_not_running);
 243         return ret;
 244 }
 245
 246 /*******************************************************************************
 247  * The automaton
 248  *******************************************************************************/
 249
 250 FSM_STATE_DECL(movement);
 251 FSM_STATE_DECL(close_to_target);
 252 FSM_STATE_DECL(wait_for_command);
 253 FSM_STATE_DECL(lost);
 254 FSM_STATE_DECL(wait_and_try_again);
 255
 256
 257 FSM_STATE(init)
 258 {
 259         if (FSM_EVENT == EV_ENTRY) {
 260                 FSM_TRANSITION(wait_for_command);
 261         }
 262 }
 263
 264 FSM_STATE(wait_for_command)
 265 {
 266         enum target_status ret;
 267         switch (FSM_EVENT) {
 268                 case EV_ENTRY:
 269                         stop();
 270                         break;
 271                 case EV_NEW_TARGET:
 272                         ret = new_target((struct move_target*)FSM_EVENT_PTR);
 273                         switch (ret) {
 274                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 275                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 276                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 277                         }
 278                         free(FSM_EVENT_PTR);
 279                         break;
 280                 default:
 281                         break;
 282         }
 283 }
 284
 285 FSM_STATE(movement)
 286 {
 287         switch (FSM_EVENT) {
 288                 case EV_ENTRY:
 289                         if  (robot.localization_works) {
 290                                 robot_pos_type p;
 291                                 ROBOT_LOCK(est_pos_uzv);
 292                                 p = robot.est_pos_uzv;
 293                                 ROBOT_UNLOCK(est_pos_uzv);
 294
 295                                 robot_set_est_pos_notrans(p.x, p.y, p.phi);
 296                         }
 297                         break;
 298                 case EV_TRAJECTORY_DONE:
 299                         // Skip close to target because sometimes it turn the robot to much
 300 //                      FSM_TRANSITION(close_to_target);
 301 //                      break;
 302                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 303                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 304                         FSM_TRANSITION(wait_for_command);
 305                         break;
 306                 case EV_OBSTACLE:
 307                         switch (recalculate_trajectory()) {
 308                                 case TARGET_OK:    break;
 309                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 310                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 311                         }
 312                         break;
 313                 case EV_OBSTACLE_BEHIND:
 314                         FSM_TRANSITION(wait_and_try_again);
 315                         break;
 316                 case EV_TRAJECTORY_LOST:
 317                         FSM_TRANSITION(lost);
 318                         break;
 319                 case EV_MOVE_STOP:
 320                         FSM_TRANSITION(wait_for_command);
 321                         break;
 322                 case EV_EXIT:
 323                         break;
 324                 case EV_NEW_TARGET:
 325                 case EV_TIMER:
 326                 case EV_RETURN:
 327                         DBG_PRINT_EVENT("unhandled event");
 328                         break;
 329         }
 330 }
 331
 332 FSM_STATE(close_to_target)
 333 {
 334         switch (FSM_EVENT) {
 335                 case EV_ENTRY:
 336                         FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
 337                         break;
 338                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 339                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 340                         FSM_TRANSITION(wait_for_command);
 341                         break;
 342                 case EV_OBSTACLE_BEHIND:
 343                         FSM_TRANSITION(wait_and_try_again);
 344                         break;
 345                 case EV_TRAJECTORY_LOST:
 346                 case EV_TIMER:
 347                         FSM_TRANSITION(lost);
 348                         break;
 349                 case EV_MOVE_STOP:
 350                         FSM_TRANSITION(wait_for_command);
 351                         break;
 352                 case EV_EXIT:
 353                         stop();
 354                         break;
 355                 case EV_OBSTACLE:
 356                 case EV_RETURN:
 357                 case EV_TRAJECTORY_DONE:
 358                 case EV_NEW_TARGET:
 359                         DBG_PRINT_EVENT("unhandled event");
 360                         break;
 361         }
 362 }
 363
 364 FSM_STATE(lost)
 365 {
 366         switch (FSM_EVENT) {
 367                 case EV_ENTRY:
 368                         stop();
 369                         FSM_TIMER(1000);
 370                         break;
 371                 case EV_TIMER:
 372                         switch (recalculate_trajectory()) {
 373                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 374                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
 375                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 376                         }
 377                         break;
 378                 case EV_MOVE_STOP:
 379                         FSM_TRANSITION(wait_for_command);
 380                         break;
 381                 case EV_EXIT:
 382                         break;
 383                 case EV_OBSTACLE_BEHIND:
 384                 case EV_TRAJECTORY_DONE:
 385                 case EV_NEW_TARGET:
 386                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 387                 case EV_TRAJECTORY_LOST:
 388                 case EV_RETURN:
 389                 case EV_OBSTACLE:
 390                         DBG_PRINT_EVENT("unhandled event");
 391         }
 392 }
 393
 394 FSM_STATE(wait_and_try_again)
 395 {
 396         switch (FSM_EVENT) {
 397                 case EV_ENTRY:
 398                         stop(); //FIXME: Not hard stop
 399                         FSM_TIMER(1000);
 400                         break;
 401                 case EV_TIMER:
 402                         switch (recalculate_trajectory()) {
 403                                 case TARGET_OK:    FSM_TRANSITION(movement); break;
 404                                 case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); printf("Inaccessible\n"); break;
 405                                 case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 406                         }
 407                         break;
 408                 case EV_TRAJECTORY_DONE:
 409                 case EV_TRAJECTORY_DONE_AND_CLOSE:
 410                         FSM_TRANSITION(wait_for_command);
 411                         FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 412                         break;
 413                 case EV_MOVE_STOP:
 414                         FSM_TRANSITION(wait_for_command);
 415                         break;
 416                 case EV_OBSTACLE_BEHIND:
 417                         FSM_TRANSITION(wait_and_try_again);
 418                         break;
 419                 case EV_NEW_TARGET:
 420                 case EV_TRAJECTORY_LOST:
 421                 case EV_RETURN:
 422                 case EV_OBSTACLE:
 423                         DBG_PRINT_EVENT("unhandled event");
 424                         break;
 425                 case EV_EXIT:
 426                         break;
 427         }
 428 }