src/robofsm/common-states.cc

   1 #define FSM_MAIN
   2 #include <robot.h>
   3 #include <fsm.h>
   4 #include <unistd.h>
   5 #include <math.h>
   6 #include <movehelper.h>
   7 #include <map.h>
   8 #include <sharp.h>
   9 #include <robomath.h>
  10 #include <string.h>
  11 #include <robodim.h>
  12 #include <error.h>
  13 #include <trgen.h>
  14 #include <stdbool.h>
  15 #include <ul_log.h>
  16 #include <shape_detect.h>
  17
  18 #include "robodata.h"
  19 #include "actuators.h"
  20 #include "match-timing.h"
  21 #include "common-states.h"
  22 #include "sub-states.h"
  23
  24 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
  25
  26 /************************************************************************
  27  * Functions used in and called from all the (almost identical)
  28  * "wait for start" states in particular strategies.
  29  ************************************************************************/
  30
  31 #undef DBG_FSM_STATE
  32 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  33                                                                    fsm->debug_name, robot_current_time(), \
  34                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  35
  36 /************************************************************************
  37  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
  38  ************************************************************************/
  39
  40 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  41
  42 /**
  43  * Vector where all absolute positions of all detected targets are stored.
  44  */
  45 std::vector<robot_pos_type> detected_target;
  46
  47 /**
  48  * Safe distance for target recognition
  49  */
  50 const double approach_radius = TARGET_RADIUS_M + 2.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M;
  51
  52 void set_initial_position()
  53 {
  54         robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, DEG2RAD(HOME_POS_ANG_DEG));
  55 }
  56
  57 void actuators_home()
  58 {
  59         //act_crane(CRANE_UP);
  60         //act_magnet(0);
  61 }
  62
  63 /* Check if the new point is within the playground scene */
  64 bool goal_is_in_playground(double goalx, double goaly)
  65 {
  66         if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
  67                 return false;
  68         else
  69                 return true;
  70 }
  71
  72 /* Check if the new point is close to the robot */
  73 bool close_goal(double goalx, double goaly)
  74 {
  75         const double close = 0.5;
  76         double x, y, phi;
  77         robot_get_est_pos(&x, &y, &phi);
  78
  79         if ((abs(goalx - x) < close) && (abs(goaly - y) < close) )
  80                 return true;
  81         else
  82                 return false;
  83 }
  84
  85 /**
  86  * Take data from hokuyo and run shape detection on it.
  87  *
  88  * Absolute positions of all detected targets centers are stored in alobal variable (vector).
  89  *
  90  * @return True if at least one target detected, else false.
  91  */
  92 static bool detect_target()
  93 {
  94         struct hokuyo_scan_type hokuyo = robot.hokuyo;
  95
  96         Shape_detect sd;
  97         std::vector<Shape_detect::Arc> arcs;
  98         sd.prepare(hokuyo.data);
  99         sd.arc_detect(arcs);
 100
 101         // clear old targets
 102         detected_target.clear();
 103
 104         if (arcs.size() > 0) {
 105                 robot_pos_type e, target, hok;
 106
 107                 robot_get_est_pos(&e.x, &e.y, &e.phi);
 108
 109                 double sinus = sin(e.phi);
 110                 double cosinus = cos(e.phi);
 111
 112                 // save absolute positions of all detected targets
 113                 for (int i = 0; i < arcs.size(); i++) {
 114                         Shape_detect::Arc *a = &arcs[i];
 115
 116                         hok.x = HOKUYO_CENTER_OFFSET_M + (double)a->center.x / 1000.0;
 117                         hok.y = (double)a->center.y / 1000.0;
 118
 119                         /* transform target position which is relative to Hokuyo
 120                         center to absolute position in space */
 121                         target.x = (hok.x * cosinus) - (hok.y * sinus) + e.x;
 122                         target.y = (hok.x * sinus) + (hok.y * cosinus) + e.y;
 123
 124                         // filter those targets not in playground range
 125                         //if (goal_is_in_playground(target.x, target.y))
 126                         //        detected_target.push_back(target);
 127                 }
 128         }
 129         return detected_target.size();
 130 }
 131
 132 /**
 133  * Calculates point to approach the target.
 134  *
 135  * @param target Position of the center of the target.
 136  * @param approach Pointer to the the intersection point of circle around
 137  * the target and line between robot center and target.
 138  */
 139 void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach)
 140 {
 141         double xrobot, yrobot, phi;
 142         double delta;
 143
 144         robot_get_est_pos(&xrobot, &yrobot, &phi);
 145
 146         delta = distance(xrobot, yrobot, xtarget, ytarget);
 147
 148         *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta);
 149         *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta);
 150
 151         *phi_approach = get_approach_angle(xtarget, ytarget);
 152 }
 153
 154 /**
 155  * Calculates point to approach the target.
 156  *
 157  * @param target Position of the center of the target.
 158  * @return Angle to approach the target form.
 159  */
 160 double get_approach_angle(double xtarget, double ytarget)
 161 {
 162         double xrobot, yrobot,phi;
 163
 164         robot_get_est_pos(&xrobot, &yrobot, &phi);
 165
 166         return atan2((ytarget - yrobot), (xtarget - xrobot));
 167 }
 168
 169
 170 /**
 171  * FSM state for neighborhood observation.
 172  *
 173  * Detect targets using shape_detect.
 174  * If no target detected, turn 120deg and try again.
 175  * Scan all 360deg and then go back to move_around state.
 176  *
 177  * If target detected, go to approach_target state.
 178  */
 179 FSM_STATE(survey)
 180 {
 181         static char turn_cntr = 0;
 182         double x, y, phi;
 183
 184         switch(FSM_EVENT) {
 185                 case EV_ENTRY:
 186                         DBG_PRINT_EVENT("survey");
 187 #if 1   // FIXME just for test
 188                         if (detect_target()) {
 189 #else
 190                         if (turn_cntr > 1) {
 191                                 robot_pos_type target;
 192                                 detected_target.clear();
 193                                 for (double i = 1; i < 5; i++) {
 194                                         target.x = i;
 195                                         target.y = i/2.0;
 196                                         detected_target.push_back(target);
 197                                 }
 198 #endif
 199                                 // target detected, go to the target
 200                                 FSM_TRANSITION(approach_target);
 201                                 DBG_PRINT_EVENT("Target detected!");
 202                         } else {
 203                                 // no target detected in this heading, turn 120°
 204                                 robot_get_est_pos(&x, &y, &phi);
 205                                 robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
 206                                 turn_cntr++;
 207                                 DBG_PRINT_EVENT("no target");
 208                         }
 209                         break;
 210                 case EV_TIMER:
 211                         if (turn_cntr > 2) {
 212                                 FSM_TRANSITION(move_around);
 213                         } else {
 214                                 FSM_TRANSITION(survey);
 215                         }
 216                         break;
 217                 case EV_MOTION_DONE:
 218                         FSM_TIMER(1000);
 219                         break;
 220                 case EV_MOTION_ERROR:
 221                         FSM_TRANSITION(move_around);
 222                         break;
 223                 case EV_EXIT:
 224                         turn_cntr = 0;
 225                         break;
 226         }
 227 }
 228
 229 /**
 230  * FSM state for approaching all detected targets.
 231  *
 232  * Try to approach target.
 233  * If approach OK - go to subautomaton and do target recognition, touch and load.
 234  * On subautomaton return check if target loaded/valid.
 235  *
 236  * If target loaded, go home.
 237  * If target not valid, try next target if any.
 238  * If approach not succesfull - go to move_around state.
 239  */
 240 FSM_STATE(approach_target)
 241 {
 242         static int target_cntr = 0;
 243         int max_target = detected_target.size();
 244         double x_target, y_tatget;
 245         double x_approach, y_approach, phi_approach;
 246
 247         switch(FSM_EVENT) {
 248                 case EV_ENTRY:
 249                         DBG_PRINT_EVENT("approaching target");
 250                         x_target = detected_target[target_cntr].x;
 251                         y_tatget = detected_target[target_cntr].y;
 252                         target_cntr++;
 253
 254                         printf("target %d / %d\n", target_cntr, max_target);
 255
 256                         get_approach_point(x_target, y_tatget, &x_approach, &y_approach, &phi_approach);
 257                         robot_goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.2), &tcFast);
 258                         break;
 259                 case EV_MOTION_DONE:
 260                         DBG_PRINT_EVENT("target approached");
 261                         SUBFSM_TRANSITION(recognize, NULL);
 262                         break;
 263                 case EV_RETURN:
 264                         if (robot.target_loaded) {
 265                                 FSM_TRANSITION(go_home);
 266                         } else if (robot.target_valid) {
 267                                 //FIXME target is valid but not loaded - try another approach direction
 268
 269                         } else if (!robot.target_valid && (target_cntr < max_target)) {
 270                                 // go for next target if any
 271                                 FSM_TRANSITION(approach_target);
 272                         } else {
 273                                 // go to new point and survey
 274                                 FSM_TRANSITION(move_around);
 275                         }
 276                         break;
 277                 case EV_MOTION_ERROR:
 278                         DBG_PRINT_EVENT("can not approach target");
 279                         if (target_cntr < max_target) {
 280                                 FSM_TRANSITION(approach_target);
 281                         } else {
 282                                 FSM_TRANSITION(move_around);
 283                         }
 284                         break;
 285                 case EV_EXIT:
 286                         target_cntr = 0;
 287                         break;
 288         }
 289 }
 290
 291 FSM_STATE(move_around)
 292 {
 293         double goalx, goaly;
 294
 295         switch (FSM_EVENT) {
 296                 case EV_ENTRY:
 297                         do {
 298                                 goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
 299                                 goaly = ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
 300                         } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
 301
 302                         robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
 303                         DBG_PRINT_EVENT("new survey point");
 304                         break;
 305                 case EV_MOTION_ERROR:
 306                         DBG_PRINT_EVENT("can not access survey point");
 307                 case EV_MOTION_DONE:
 308                         FSM_TRANSITION(survey);
 309                         break;
 310                 case EV_TIMER:
 311                         break;
 312                 case EV_EXIT:
 313                         break;
 314         }
 315 }
 316
 317 FSM_STATE(go_home)
 318 {
 319         switch (FSM_EVENT) {
 320                 case EV_ENTRY:
 321                         DBG_PRINT_EVENT("homing");
 322                         robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcSlow);
 323                         break;
 324                 case EV_MOTION_ERROR:
 325                         DBG_PRINT_EVENT("ERROR: home position is not reachable!");
 326                         FSM_TIMER(1000);
 327                         break;
 328                 case EV_TIMER:
 329                         FSM_TRANSITION(go_home);
 330                         break;
 331                 case EV_MOTION_DONE:
 332                 case EV_EXIT:
 333                         DBG_PRINT_EVENT("Mission completed!");
 334                         robot_exit();
 335                         break;
 336         }
 337 }
 338
 339 /*
 340 FSM_STATE()
 341 {
 342         switch(FSM_EVENT) {
 343                 case EV_ENTRY:
 344                         break;
 345                 case EV_START:
 346                 case EV_TIMER:
 347                 case EV_RETURN:
 348                 case EV_CRANE_DONE:
 349                 case EV_MOTION_DONE:
 350                 case EV_MOTION_ERROR:
 351                 case EV_SWITCH_STRATEGY:
 352                         DBG_PRINT_EVENT("unhandled event");
 353                 case EV_EXIT:
 354                         break;
 355         }
 356 }
 357 */