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