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                 return true;
 129         } else {
 130                 return false;
 131         }
 132 }
 133
 134 /**
 135  * Calculates point to approach the target.
 136  *
 137  * @param target Position of the center of the target.
 138  * @param approach Pointer to the the intersection point of circle around
 139  * the target and line between robot center and target.
 140  */
 141 void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach)
 142 {
 143         double xrobot, yrobot, phi;
 144         double delta;
 145
 146         robot_get_est_pos(&xrobot, &yrobot, &phi);
 147
 148         delta = distance(xrobot, yrobot, xtarget, ytarget);
 149
 150         *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta);
 151         *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta);
 152
 153         *phi_approach = get_approach_angle(xtarget, ytarget);
 154 }
 155
 156 /**
 157  * Calculates point to approach the target.
 158  *
 159  * @param target Position of the center of the target.
 160  * @return Angle to approach the target form.
 161  */
 162 double get_approach_angle(double xtarget, double ytarget)
 163 {
 164         double xrobot, yrobot,phi;
 165
 166         robot_get_est_pos(&xrobot, &yrobot, &phi);
 167
 168         return atan2((ytarget - yrobot), (xtarget - xrobot));
 169 }
 170
 171
 172 /**
 173  * FSM state for neighborhood observation.
 174  *
 175  * Detect targets using shape_detect.
 176  * If no target detected, turn 120deg and try again.
 177  * Scan all 360deg and then go back to move_around state.
 178  *
 179  * If target detected, go to approach_target state.
 180  */
 181 FSM_STATE(survey)
 182 {
 183         static char turn_cntr = 0;
 184         double x, y, phi;
 185
 186         switch(FSM_EVENT) {
 187                 case EV_ENTRY:
 188                         DBG_PRINT_EVENT("survey");
 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                                 // target detected, go to the target
 198                                 FSM_TRANSITION(approach_target);
 199                                 DBG_PRINT_EVENT("Target detected!");
 200                         } else {
 201                                 // no target detected in this heading, turn 120°
 202                                 robot_get_est_pos(&x, &y, &phi);
 203                                 robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
 204                                 turn_cntr++;
 205                                 DBG_PRINT_EVENT("no target");
 206                         }
 207                         break;
 208                 case EV_TIMER:
 209                         if (turn_cntr > 2) {
 210                                 FSM_TRANSITION(move_around);
 211                         } else {
 212                                 FSM_TRANSITION(survey);
 213                         }
 214                         break;
 215                 case EV_MOTION_DONE:
 216                         FSM_TIMER(1000);
 217                         break;
 218                 case EV_START:
 219                 case EV_RETURN:
 220                 case EV_JAWS_DONE:
 221                 case EV_SWITCH_STRATEGY:
 222                         DBG_PRINT_EVENT("unhandled event!");
 223                         break;
 224                 case EV_MOTION_ERROR:
 225                         FSM_TRANSITION(move_around);
 226                         break;
 227                 case EV_EXIT:
 228                         turn_cntr = 0;
 229                         break;
 230         }
 231 }
 232
 233 /**
 234  * FSM state for approaching all detected targets.
 235  *
 236  * Try to approach target.
 237  * If approach OK - go to subautomaton and do target recognition, touch and load.
 238  * On subautomaton return check if target loaded/valid.
 239  *
 240  * If target loaded, go home.
 241  * If target not valid, try next target if any.
 242  * If approach not succesfull - go to move_around state.
 243  */
 244 FSM_STATE(approach_target)
 245 {
 246         static int target_cntr = 0;
 247         int max_target = detected_target.size();
 248         double x_target, y_tatget;
 249         double x_approach, y_approach, phi_approach;
 250
 251         switch(FSM_EVENT) {
 252                 case EV_ENTRY:
 253                         DBG_PRINT_EVENT("approaching target");
 254                         x_target = detected_target[target_cntr].x;
 255                         y_tatget = detected_target[target_cntr].y;
 256                         target_cntr++;
 257
 258                         get_approach_point(x_target, y_tatget, &x_approach, &y_approach, &phi_approach);
 259                         robot_goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.2), &tcSlow);
 260                         break;
 261                 case EV_MOTION_DONE:
 262                         DBG_PRINT_EVENT("target approached");
 263                         SUBFSM_TRANSITION(recognize, NULL);
 264                         break;
 265                 case EV_RETURN:
 266                         if (robot.target_loaded) {
 267                                 FSM_TRANSITION(go_home);
 268                         } else if (robot.target_valid) {
 269                                 //FIXME target is valid but not loaded - try another approach direction
 270
 271                         } else if (!robot.target_valid && (target_cntr < max_target)) {
 272                                 // go for next target if any
 273                                 FSM_TRANSITION(approach_target);
 274                         } else {
 275                                 // go to new point and survey
 276                                 FSM_TRANSITION(move_around);
 277                         }
 278                         break;
 279                 case EV_MOTION_ERROR:
 280                         DBG_PRINT_EVENT("ERROR approaching");
 281                         if (target_cntr < max_target) {
 282                                 FSM_TRANSITION(approach_target);
 283                         } else {
 284                                 FSM_TRANSITION(move_around);
 285                         }
 286                         break;
 287                 case EV_TIMER:
 288                 case EV_START:
 289                 case EV_JAWS_DONE:
 290                 case EV_SWITCH_STRATEGY:
 291                         DBG_PRINT_EVENT("unhandled event");
 292                         break;
 293                 case EV_EXIT:
 294                         target_cntr = 0;
 295                         break;
 296         }
 297 }
 298
 299 FSM_STATE(move_around)
 300 {
 301         double goalx, goaly, phi;
 302         static int survey_cnt = 0;
 303
 304         switch (FSM_EVENT) {
 305                 case EV_ENTRY:
 306
 307                         do {
 308                                 goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
 309                                 goaly = ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
 310                         } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
 311
 312                         robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
 313                         DBG_PRINT_EVENT("new survey point");
 314                         survey_cnt++;
 315                         break;
 316                 case EV_MOTION_ERROR:
 317                         DBG_PRINT_EVENT("ERROR: can not access survey point");
 318                 case EV_MOTION_DONE:
 319                         FSM_TRANSITION(survey);
 320                         break;
 321                 case EV_TIMER:
 322                         break;
 323                 case EV_RETURN:
 324                         break;
 325                 case EV_START:
 326                         /* do nothing */
 327                         break;
 328                 case EV_EXIT:
 329                         //ShmapFree();
 330                         break;
 331                 default:
 332                         break;
 333         }
 334 }
 335
 336 FSM_STATE(go_home)
 337 {
 338         switch (FSM_EVENT) {
 339                 case EV_ENTRY:
 340                         DBG_PRINT_EVENT("homing");
 341                         robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcSlow);
 342                         break;
 343                 case EV_MOTION_ERROR:
 344                         DBG_PRINT_EVENT("ERROR: home position is not reachable!");
 345                         FSM_TIMER(1000);
 346                         break;
 347                 case EV_MOTION_DONE:
 348                         break;
 349                 case EV_TIMER:
 350                         FSM_TRANSITION(go_home);
 351                         break;
 352                 case EV_START:
 353                         /* do nothing */
 354                         break;
 355                 case EV_EXIT:
 356                         break;
 357                 default:
 358                         DBG_PRINT_EVENT("Mission completed!");
 359                         robot_exit();
 360                         break;
 361         }
 362 }
 363
 364 /*
 365 FSM_STATE()
 366 {
 367         switch(FSM_EVENT) {
 368                 case EV_ENTRY:
 369                         break;
 370                 case EV_START:
 371                 case EV_TIMER:
 372                 case EV_RETURN:
 373                 case EV_CRANE_DONE:
 374                 case EV_MOTION_DONE:
 375                 case EV_MOTION_ERROR:
 376                 case EV_SWITCH_STRATEGY:
 377                         DBG_PRINT_EVENT("unhandled event");
 378                 case EV_EXIT:
 379                         break;
 380         }
 381 }
 382 */