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