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