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 #include <sys/time.h>
  10 #include <time.h>
  11 #include "trgen.h"
  12 #include "balet.h"
  13 #include "robodata.h"
  14 #include "robot.h"
  15 #include <pthread.h>
  16 #include <path_planner.h>
  17 #include <signal.h>
  18 #include <movehelper.h>
  19 #include <sharp.h>
  20 #include <unistd.h>
  21 #include <map.h>
  22 #include "robot_config.h"
  23 #include <robomath.h>
  24 #include "motion-control.h"
  25 #include <hokuyo.h>
  26 #include <ul_log.h>
  27 #include "timedFSM.h"
  28 #include <boost/mpl/list.hpp>
  29 #include <boost/statechart/transition.hpp>
  30 #include <boost/statechart/custom_reaction.hpp>
  31 //#include "fsmhead.hpp"
  32
  33 UL_LOG_CUST(ulogd_fsmmove); /* Log domain name = ulogd + name of the file */
  34
  35 #ifdef MOTION_DEBUG
  36    #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
  37 #else
  38    #define DBG(format, ...)
  39 #endif
  40
  41 #define MAX_WAIT_FOR_CLOSE_MS 2000
  42
  43 // time to calculate new trajectory [seconds]
  44 #define TRGEN_DELAY 0.3
  45
  46
  47 // Target position of the current movement
  48 static struct move_target current_target;
  49
  50 enum target_status {
  51         TARGET_OK,    // We can go the the target
  52         TARGET_INACC, // Target is inaccessible because of an obstacle
  53         TARGET_ERROR  // Fatal errror during planning
  54 };
  55
  56 // we may need something similar in future
  57 void invalidate(Point point)
  58 {
  59         //ShmapSetRectangleFlag(x-s, y-s, x+s, y+s, MAP_FLAG_INVALIDATE_WALL, 0)
  60 }
  61
  62 static double free_space_in_front_of_robot()
  63 {
  64         int i, i1, i2;
  65         int min = 10000, m;
  66         i1 = HOKUYO_DEG_TO_INDEX(-45);
  67         i2 = HOKUYO_DEG_TO_INDEX(+45);
  68         for (i=MIN(i1, i2); i < MAX(i1, i2); i++) {
  69                 m = robot.orte.hokuyo_scan.data[i];
  70                 if (m > 19 && m < min)
  71                         min = m;
  72         }
  73         return min/1000.0 - HOKUYO_CENTER_OFFSET_M;;
  74 }
  75
  76 static bool obstackle_in_front_if_turn(Trajectory *t)
  77 {
  78         double f;
  79         bool ret;
  80
  81         f = free_space_in_front_of_robot();
  82
  83         if (fabs(t->initialTurnBy) < DEG2RAD(95) &&
  84             f > 1.2*(ROBOT_WIDTH_M/2.0)) {
  85                 ret = false;
  86         } else if (f > 1.5*ROBOT_AXIS_TO_BACK_M) {
  87                 ret = false;
  88         } else
  89                 ret = true;
  90
  91         return ret;
  92 }
  93
  94 /**
  95  * @brief       FIXME Calculates and simplifies a path to goal position avoiding obstacles.
  96  * @param start_in_future is delay utilised to calculate the trajectory while robot
  97  * is moving (zero means to start movement from the current position)
  98  * @return      True on succes, false on error.
  99  */
 100 static enum target_status new_goal(struct move_target *move_target, double start_in_future)
 101 {
 102         enum target_status ret;
 103         double angle;
 104         PathPoint * path = NULL;
 105         PathPoint * tmp_point = NULL;
 106         Pos future_traj_point;
 107         Point target(move_target->x, move_target->y);
 108         Point center(PLAYGROUND_WIDTH_M/2.0, PLAYGROUND_HEIGHT_M/2.0);
 109         double time_ts;
 110         bool backward = false;
 111
 112         // Where to start trajectory planning?
 113         get_future_pos(start_in_future, future_traj_point, time_ts);
 114         Point start(future_traj_point.x, future_traj_point.y);
 115
 116         Trajectory *t = new Trajectory(move_target->tc, backward);
 117
 118         /*
 119         // Clear all invalidate flags;
 120         ShmapSetRectangleFlag(0, 0, PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M, 0, MAP_FLAG_INVALIDATE_WALL);
 121         // Set invalidate flags if we are going to walled area
 122         if (false) // we may need this in future
 123                 invalidate(start);
 124         */
 125
 126         if (move_target->heading.operation == TOP_ARRIVE_FROM) {
 127                 robot.move_helper.get_arrive_from_point(move_target->x, move_target->y, move_target->heading,
 128                                       &target.x, &target.y);
 129         }
 130
 131         if (move_target->use_planning) {
 132                 /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
 133                 switch (path_planner(start.x, start.y, target.x, target.y, &path, &angle)) {
 134                         case PP_ERROR_MAP_NOT_INIT:
 135                                 ret = TARGET_ERROR; break;
 136                         case PP_ERROR_GOAL_IS_OBSTACLE:
 137                         case PP_ERROR_GOAL_NOT_REACHABLE:
 138                                 ret = TARGET_INACC; break;
 139                         default:
 140                                 ret = TARGET_OK; break;
 141                 }
 142
 143                 if (ret != TARGET_OK)
 144                         return ret;
 145                 // Add this  path to the trajectory.
 146                 for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) {
 147                         DBG("ADDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y);
 148                         t->addPoint(tmp_point->x, tmp_point->y);
 149                 }
 150                 freePathMemory(path);
 151         } else {
 152                 t->addPoint(target.x, target.y);
 153         }
 154
 155         if (move_target->heading.operation == TOP_ARRIVE_FROM) {
 156                 t->addPoint(move_target->x, move_target->y);
 157         }
 158         t->finalHeading = target2final_heading(move_target->heading);
 159
 160         if (t->prepare(future_traj_point)) {
 161                 if (obstackle_in_front_if_turn(t))
 162                         ret = TARGET_INACC;
 163                 else {
 164                         if (start_in_future) {
 165                                 /* not to switch immediately
 166                                    switch_trajectory_at(t, time_ts); */
 167                                 // new test - appending trajectories
 168                                 go(t, time_ts);
 169                         } else {
 170                                 go(t, 0);
 171                         }
 172                 }
 173         } else {
 174                 delete(t);
 175                 ret = TARGET_ERROR;
 176         }
 177
 178         return ret;
 179 }
 180
 181
 182 /**
 183  * Starts moving on trajectory prepared by functions in
 184  * movehelper.cc. In case of error it sends the proper event to MAIN
 185  * FSM.
 186  *
 187  * @return TARGET_OK on succes, TARGET_ERROR on error.
 188  */
 189 static enum target_status new_trajectory(Trajectory *t)
 190 {
 191         Pos pos;
 192
 193         if (!t) {
 194                 ul_logerr("ERROR: No trajectory\n");
 195                 return TARGET_ERROR;
 196         }
 197         // FIXME: This is duplicite code with new_goal. Remove this
 198         // function and use always new_goal.
 199         robot.get_est_pos(pos.x, pos.y, pos.phi);
 200         pos.v = 0;
 201         pos.omega = 0;
 202
 203         if (t->prepare(pos)) {
 204                 if (robot.check_turn_safety && obstackle_in_front_if_turn(t)) {
 205                         return TARGET_INACC;
 206                 }
 207                 else
 208                         go(t, 0);
 209                 return TARGET_OK;
 210         } else {
 211                 delete(t);
 212                 return TARGET_ERROR;
 213         }
 214 }
 215
 216 /**
 217  * Handles new target signal from main FSM
 218  *
 219  * @param target
 220  *
 221  * @return
 222  */
 223 static enum target_status new_target(struct move_target *target)
 224 {
 225         enum target_status ret;
 226         if (target->trajectory) {
 227                 Trajectory *t = (Trajectory*)target->trajectory;
 228                 target->tc = t->constr;
 229                 ret = new_trajectory(t);
 230                 target->trajectory = NULL;
 231                 // Trajectory is deleted by somebody else
 232         } else {
 233                 ret = new_goal(target, 0);
 234         }
 235         if (ret != TARGET_ERROR) {
 236                 current_target = *target;
 237         }
 238         return ret;
 239 }
 240
 241 /**
 242  * Recalculates trajectory to previous move target specified by
 243  * new_target()
 244  *
 245  * @return
 246  */
 247 enum target_status
 248 recalculate_trajectory(void)
 249 {
 250         /* TODO: Different start for planning than esitmated position */
 251         enum target_status ret;
 252         current_target.use_planning = true;
 253         ret = new_goal(&current_target, TRGEN_DELAY);
 254         switch (ret) {
 255         case TARGET_OK:
 256                 break;
 257         case TARGET_ERROR:
 258                 ul_logerr("Target error on recalculation_in_progress\n");
 259                 break;
 260         case TARGET_INACC:
 261                 break;
 262         }
 263
 264         int val;
 265         sem_getvalue(&recalculation_not_running, &val);
 266         if (val == 0)
 267                 sem_post(&recalculation_not_running);
 268         return ret;
 269 }
 270
 271 /*******************************************************************************
 272  * The automaton
 273  *******************************************************************************/
 274
 275 struct movement;
 276 struct close_to_target;
 277 struct wait_for_command;
 278 struct lost;
 279 struct wait_and_try_again;
 280 struct MotionBase;
 281
 282 struct FSMMotion : boost::statechart::asynchronous_state_machine<FSMMotion, MotionBase, Scheduler> {
 283         FSMMotion(my_context ctx) : my_base(ctx) {
 284                 printf("%s\n", __FUNCTION__);
 285         }
 286 };
 287
 288 struct MotionBase : sc::simple_state<MotionBase, FSMMotion, wait_for_command> {
 289         sc::result react(const EV_TRAJECTORY_DONE &) {
 290 //              DBG_PRINT_EVENT("unhandled event");
 291                 return discard_event();
 292         }
 293         sc::result react(const EV_TRAJECTORY_DONE_AND_CLOSE &) {
 294 //              DBG_PRINT_EVENT("unhandled event");
 295                 return discard_event();
 296         }
 297         sc::result react(const EV_TRAJECTORY_LOST &) {
 298 //              DBG_PRINT_EVENT("unhandled event");
 299                 return discard_event();
 300         }
 301         sc::result react(const EV_MOVE_STOP &) {
 302 //              DBG_PRINT_EVENT("unhandled event");
 303                 return discard_event();
 304         }
 305         sc::result react(const EV_OBSTACLE &) {
 306 //              DBG_PRINT_EVENT("unhandled event");
 307                 return discard_event();
 308         }
 309         sc::result react(const EV_OBSTACLE_BEHIND &) {
 310 //              DBG_PRINT_EVENT("unhandled event");
 311                 return discard_event();
 312         }
 313         sc::result react(const EV_OBSTACLE_SIDE &) {
 314 //              DBG_PRINT_EVENT("unhandled event");
 315                 return discard_event();
 316         }
 317         sc::result react(const EV_RETURN &) {
 318 //              DBG_PRINT_EVENT("unhandled event");
 319                 return discard_event();
 320         }
 321         sc::result react(const EV_NEW_TARGET &) {
 322 //              DBG_PRINT_EVENT("unhandled event");
 323                 return discard_event();
 324         }
 325         typedef mpl::list<
 326                 sc::custom_reaction<EV_TRAJECTORY_DONE>,
 327                 sc::custom_reaction<EV_TRAJECTORY_DONE_AND_CLOSE>,
 328                 sc::custom_reaction<EV_TRAJECTORY_LOST>,
 329                 sc::custom_reaction<EV_MOVE_STOP>,
 330                 sc::custom_reaction<EV_OBSTACLE_SIDE>,
 331                 sc::custom_reaction<EV_OBSTACLE_BEHIND>,
 332                 sc::custom_reaction<EV_OBSTACLE>,
 333                 sc::custom_reaction<EV_RETURN>,
 334                 sc::custom_reaction<EV_NEW_TARGET> > reactions;
 335 };
 336
 337 struct wait_for_command : TimedSimpleState<wait_for_command, MotionBase>
 338 {
 339         enum target_status ret;
 340         wait_for_command() {
 341                 printf("%s\n", __FUNCTION__);
 342                 stop();
 343         }
 344         sc::result react(const EV_NEW_TARGET &event) {
 345                 printf("%s EV_NEW_TARGET\n", __FUNCTION__);
 346                 ret = new_target((struct move_target*)event.ev_ptr);
 347                 free (event.ev_ptr);
 348                 switch (ret) {
 349                         case TARGET_OK: return transit<movement>();
 350                         case TARGET_INACC: return transit<wait_and_try_again>();
 351                         case TARGET_ERROR:
 352                           /*FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL)*/;
 353                           ul_logerr("Target error\n");
 354                 }
 355                 return discard_event();
 356         }
 357         typedef sc::custom_reaction<EV_NEW_TARGET> reactions;
 358 };
 359
 360 struct movement : TimedSimpleState<movement, MotionBase>
 361 {
 362         sc::result react(const EV_TRAJECTORY_DONE &) {
 363                 // Skip close to target because sometimes it turn the robot to much
 364                 // FSM_TRANSITION(close_to_target);
 365                 robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
 366                 /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
 367                 return transit<wait_for_command>();
 368         }
 369         sc::result react(const EV_TRAJECTORY_DONE_AND_CLOSE &) {
 370
 371                 robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
 372                 /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
 373                 return transit<wait_for_command>();
 374         }
 375         sc::result react(const EV_OBSTACLE &) {
 376                 switch (recalculate_trajectory()) {
 377                         // We can go to the target:
 378                         case TARGET_OK: return discard_event();
 379                         // Target inaccessible because of an obstacle
 380                         case TARGET_INACC: return transit<wait_and_try_again>();
 381                         // Fatal error during planning
 382                         case TARGET_ERROR:
 383                           /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
 384                            ul_logerr("Target error\n");
 385                            /* FIXME (Filip): shouldn't transition to wait_for_command be here?
 386                              No, probably not, if an error occurs, robot won't move any more
 387                               and we can recognize the error. */
 388                 }
 389                 return discard_event();
 390         }
 391         sc::result react(const EV_OBSTACLE_BEHIND &) {
 392                 if (robot.moves_backward && robot.use_back_bumpers &&
 393                     (robot.orte.robot_bumpers.bumper_left_across ||
 394                     robot.orte.robot_bumpers.bumper_right_across ||
 395                     robot.orte.robot_bumpers.bumper_rear_left ||
 396                     robot.orte.robot_bumpers.bumper_rear_right))
 397                         return transit<wait_and_try_again>();
 398                 return discard_event();
 399         }
 400         sc::result react(const EV_OBSTACLE_SIDE &) {
 401                 if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
 402                     (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
 403                         return transit<wait_and_try_again>();
 404                 return discard_event();
 405         }
 406         typedef mpl::list<
 407                 sc::transition<EV_TRAJECTORY_LOST, lost>,
 408                 sc::transition<EV_MOVE_STOP, wait_for_command>,
 409                 sc::custom_reaction<EV_OBSTACLE_SIDE>,
 410                 sc::custom_reaction<EV_OBSTACLE_BEHIND>,
 411                 sc::custom_reaction<EV_OBSTACLE>,
 412                 sc::custom_reaction<EV_TRAJECTORY_DONE>,
 413                 sc::custom_reaction<EV_TRAJECTORY_DONE_AND_CLOSE> > reactions;
 414 };
 415
 416 struct close_to_target : TimedState<close_to_target, MotionBase>{
 417         Timer timeout_for_close;
 418         close_to_target(my_context ctx) : base_state(ctx) {
 419                 runTimer(timeout_for_close, MAX_WAIT_FOR_CLOSE_MS, new EV_TIMER());
 420         }
 421         ~close_to_target() {
 422                 stop();
 423         }
 424         sc::result react(const EV_TRAJECTORY_DONE_AND_CLOSE &) {
 425                 /*TODO  FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
 426                 return transit<wait_for_command>();
 427         }
 428         sc::result react(const EV_OBSTACLE_BEHIND &) {
 429                 if (robot.moves_backward && robot.use_back_bumpers &&
 430                     (robot.orte.robot_bumpers.bumper_left_across ||
 431                     robot.orte.robot_bumpers.bumper_right_across ||
 432                     robot.orte.robot_bumpers.bumper_rear_left ||
 433                     robot.orte.robot_bumpers.bumper_rear_right))
 434                     return transit<wait_and_try_again>();
 435                 return discard_event();
 436         }
 437         sc::result react(const EV_OBSTACLE_SIDE &) {
 438                 if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
 439                     (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
 440                         return transit<wait_and_try_again>();
 441                 return discard_event();
 442         }
 443         typedef mpl::list<
 444                 sc::custom_reaction<EV_TRAJECTORY_DONE_AND_CLOSE>,
 445                 sc::custom_reaction<EV_OBSTACLE_BEHIND>,
 446                 sc::custom_reaction<EV_OBSTACLE_SIDE>,
 447                 sc::transition<EV_TRAJECTORY_LOST, lost>,
 448                 sc::transition<EV_TIMER, lost>,
 449                 sc::transition<EV_MOVE_STOP, wait_for_command> > reactions;
 450 };
 451
 452 struct lost : TimedState<lost, MotionBase>{
 453         Timer lost_tout;
 454         lost(my_context ctx) : base_state(ctx) {
 455                 stop();
 456                 runTimer(lost_tout, 1000, new EV_TIMER());
 457         }
 458         sc::result react(const EV_TIMER &) {
 459                 switch (recalculate_trajectory()) {
 460                         case TARGET_OK:    return transit<movement>();
 461                         case TARGET_INACC: return transit<wait_and_try_again>();
 462                         case TARGET_ERROR:
 463                             /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
 464                             ul_logerr("Target error\n");
 465                 }
 466                 return discard_event();
 467         }
 468         typedef mpl::list<
 469                 sc::custom_reaction<EV_TIMER>,
 470                 sc::transition<EV_MOVE_STOP, wait_for_command>  > reactions;
 471 };
 472
 473 struct wait_and_try_again : TimedState<wait_and_try_again, MotionBase> {
 474         Timer wait_tout;
 475         wait_and_try_again(my_context ctx) : base_state(ctx) {
 476                 stop();
 477                 runTimer(wait_tout, 1000, new EV_TIMER());
 478         }
 479         sc::result react(const EV_TIMER &) {
 480                 switch (recalculate_trajectory()) {
 481                         // We can go to the target:
 482                         case TARGET_OK: return transit<movement>();
 483                         // Target inaccessible because of an obstacle
 484                         case TARGET_INACC:
 485                                 ul_logerr("Inaccessible\n");
 486                                 return transit<wait_and_try_again>();
 487                                 /* FIXME (Filip): this could happen forever */
 488                         // Fatal error during planning
 489                         case TARGET_ERROR:
 490                                 /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
 491                                 ul_logerr("Target error\n");
 492                 }
 493                 return discard_event();
 494         }
 495         sc::result react(const EV_TRAJECTORY_DONE &) {
 496                 /* TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
 497                 return transit<wait_for_command>();
 498         }
 499         sc::result react(const EV_TRAJECTORY_DONE_AND_CLOSE &) {
 500                 /* TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
 501                 return transit<wait_for_command>();
 502         }
 503         typedef mpl::list<
 504                 sc::custom_reaction<EV_TIMER>,
 505                 sc::custom_reaction<EV_TRAJECTORY_DONE>,
 506                 sc::custom_reaction<EV_TRAJECTORY_DONE_AND_CLOSE>,
 507                 sc::transition<EV_MOVE_STOP, wait_for_command>
 508         > reactions;
 509 };