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