src/robofsm/common-states.h

   1 #ifndef COMMON_STATES_H
   2 #define COMMON_STATES_H
   3
   4
   5 #include "fsm_top.h"
   6 #include <robot.h>
   7 #include <unistd.h>
   8 #include <math.h>
   9 #include <movehelper.h>
  10 #include <map.h>
  11 #include <sharp.h>
  12 #include <robomath.h>
  13 #include <string.h>
  14 #include <robodim.h>
  15 #include <error.h>
  16 #include <trgen.h>
  17 #include <stdbool.h>
  18 #include <ul_log.h>
  19 #include <shape_detect.h>
  20
  21 #include "robodata.h"
  22 #include "actuators.h"
  23 #include "state_defines.h"
  24
  25
  26 std::vector<robot_pos_type> detected_target;
  27
  28 /**
  29  * Safe distance for target recognition
  30  */
  31 const double approach_radius = TARGET_RADIUS_M + 3.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M;
  32
  33 static bool detect_target();
  34 bool goal_is_in_playground(double goalx, double goaly);
  35 bool close_goal(double goalx, double goaly);
  36 void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach);
  37 double get_approach_angle(double xtarget, double ytarget);
  38
  39 bool goal_is_in_playground(double goalx, double goaly)
  40 {
  41         if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
  42                 return false;
  43         else
  44                 return true;
  45 }
  46
  47 /* Check if the new point is close to the robot */
  48 bool close_goal(double goalx, double goaly)
  49 {
  50         const double close = 0.5;
  51         double x, y, phi;
  52         robot.get_est_pos(x, y, phi);
  53
  54         if ((abs(goalx - x) < close) && (abs(goaly - y) < close) )
  55                 return true;
  56         else
  57                 return false;
  58 }
  59
  60 /**
  61  * Take data from hokuyo and run shape detection on it.
  62  *
  63  * Absolute positions of all detected targets centers are stored in alobal variable (vector).
  64  *
  65  * @return True if at least one target detected, else false.
  66  */
  67 static bool detect_target()
  68 {
  69         struct hokuyo_scan_type hokuyo = robot.hokuyo;
  70
  71         Shape_detect sd;
  72         std::vector<Shape_detect::Arc> arcs;
  73         sd.prepare(hokuyo.data);
  74         sd.arc_detect(arcs);
  75
  76         // clear old targets
  77         detected_target.clear();
  78
  79         if (arcs.size() > 0) {
  80                 robot_pos_type e, target, hok;
  81
  82                 robot.get_est_pos(e.x, e.y, e.phi);
  83
  84                 double sinus = sin(e.phi);
  85                 double cosinus = cos(e.phi);
  86
  87                 // save absolute positions of all detected targets
  88                 for (int i = 0; i < arcs.size(); i++) {
  89                         Shape_detect::Arc *a = &arcs[i];
  90
  91                         hok.x = HOKUYO_CENTER_OFFSET_M + (double)a->center.x / 1000.0;
  92                         hok.y = (double)a->center.y / 1000.0;
  93
  94                         /* transform target position which is relative to Hokuyo
  95                         center to absolute position in space */
  96                         target.x = (hok.x * cosinus) - (hok.y * sinus) + e.x;
  97                         target.y = (hok.x * sinus) + (hok.y * cosinus) + e.y;
  98
  99                         // filter those targets not in playground range
 100                         if (goal_is_in_playground(target.x, target.y))
 101                                 detected_target.push_back(target);
 102                 }
 103         }
 104         return detected_target.size();
 105 }
 106
 107 /**
 108  * Calculates point to approach the target.
 109  *
 110  * @param target Position of the center of the target.
 111  * @param approach Pointer to the the intersection point of circle around
 112  * the target and line between robot center and target.
 113  */
 114 void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach)
 115 {
 116         double xrobot, yrobot, phi;
 117         double delta;
 118
 119         robot.get_est_pos(xrobot, yrobot, phi);
 120
 121         delta = distance(xrobot, yrobot, xtarget, ytarget);
 122
 123         *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta);
 124         *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta);
 125
 126         *phi_approach = get_approach_angle(xtarget, ytarget);
 127 }
 128
 129 /**
 130  * Calculates point to approach the target.
 131  *
 132  * @param target Position of the center of the target.
 133  * @return Angle to approach the target form.
 134  */
 135 double get_approach_angle(double xtarget, double ytarget)
 136 {
 137         double xrobot, yrobot,phi;
 138
 139         robot.get_est_pos(xrobot, yrobot, phi);
 140
 141         return atan2((ytarget - yrobot), (xtarget - xrobot));
 142 }
 143
 144
 145 /**
 146  * FSM state for neighborhood observation.
 147  *
 148  * Detect targets using shape_detect.
 149  * If no target detected, turn 120deg and try again.
 150  * Scan all 360deg and then go back to move_around state.
 151  *
 152  * If target detected, go to approach_target state.
 153  */
 154 struct survey : TimedSimpleState<survey, competing> {
 155         char turn_cntr;
 156         double x, y, phi;
 157         Timer t;
 158         survey() {
 159                 turn_cntr = 0;
 160                 do_work();
 161         }
 162         ~survey() {
 163                 turn_cntr = 0;
 164         }
 165         sc::result react(const evTimer& ) {
 166                 if (turn_cntr > 2) {
 167                         return transit<move_around>();
 168                 } else {
 169                         do_work();
 170                         return discard_event();
 171                 }
 172         }
 173         sc::result react(const evMotionDone&) {
 174                 runTimer(t,1000, new evTimer());
 175                 return discard_event();
 176         }
 177
 178         typedef mpl::list<
 179                 sc::custom_reaction<evTimer>,
 180                 sc::custom_reaction<evMotionDone>,
 181                 sc::transition<evMotionError, move_around>,
 182                 sc::transition<evTargetDetected, approach_target> > reactions;
 183       void do_work() {
 184         //              DBG_PRINT_EVENT("survey");
 185 #if 1   // FIXME just for test
 186                 if (detect_target()) {
 187 #else
 188                 if (turn_cntr > 1) {
 189                         robot_pos_type target;
 190                         detected_target.clear();
 191                         for (double i = 1; i < 5; i++) {
 192                                 target.x = i;
 193                                 target.y = i/2.0;
 194                                 detected_target.push_back(target);
 195                         }
 196 #endif
 197                         // target detected, go to the target
 198                         robot.sched.queue_event(robot.MAIN, new evTargetDetected());
 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.move_helper.goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
 204                         turn_cntr++;
 205 //                      DBG_PRINT_EVENT("no target");
 206                 }
 207       }
 208 };
 209
 210 /**
 211  * FSM state for approaching all detected targets.
 212  *
 213  * Try to approach target.
 214  * If approach OK - go to subautomaton and do target recognition, touch and load.
 215  * On subautomaton return check if target loaded/valid.
 216  *
 217  * If target loaded, go home.
 218  * If target not valid, try next target if any.
 219  * If approach not succesfull - go to move_around state.
 220  */
 221 struct approach_target : TimedSimpleState<approach_target, competing, recognize_init> {
 222         int target_cntr;
 223         int max_target;
 224         double x_target, y_tatget;
 225         double x_approach, y_approach, phi_approach;
 226
 227         approach_target() {
 228 //              DBG_PRINT_EVENT("approaching target");
 229                 target_cntr = 0;
 230                 do_work();
 231         }
 232         ~approach_target() {
 233               target_cntr = 0;
 234         }
 235         sc::result react(const evMotionDone&) {
 236 //              DBG_PRINT_EVENT("target approached");
 237                 robot.sched.queue_event(robot.MAIN, new evRunSubFSM());
 238                 return discard_event();
 239         }
 240         sc::result react(const evReturn&) {
 241                 if (robot.target_loaded) {
 242                         return transit<go_home>();
 243                 } else if (robot.target_valid) {
 244                         //FIXME target is valid but not loaded - try another approach direction
 245                         return discard_event();
 246                 } else if (!robot.target_valid && (target_cntr < max_target)) {
 247                         // go for next target if any
 248                         do_work();
 249                         return discard_event();
 250                 } else {
 251                         // go to new point and survey
 252                         return transit<move_around>();
 253                 }
 254         }
 255         sc::result react (const evMotionError&) {
 256 //              DBG_PRINT_EVENT("can not approach target");
 257                 if (target_cntr < max_target) {
 258                         return transit<approach_target>();
 259                 } else {
 260                         return transit<move_around>();
 261                 }
 262         }
 263         typedef mpl::list<
 264                 sc::custom_reaction<evMotionDone>,
 265                 sc::custom_reaction<evReturn>,
 266                 sc::custom_reaction<evMotionError> > reactions;
 267         void do_work() {
 268                 max_target = detected_target.size();
 269                 x_target = detected_target[target_cntr].x;
 270                 y_tatget = detected_target[target_cntr].y;
 271                 target_cntr++;
 272                 printf("target %d / %d\n", target_cntr, max_target);
 273
 274                 get_approach_point(x_target, y_tatget, &x_approach, &y_approach, &phi_approach);
 275                 robot.move_helper.goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.1), &tcSlow);
 276         }
 277 };
 278
 279 struct move_around : TimedSimpleState<move_around, competing> {
 280         double goalx, goaly;
 281         move_around() {
 282                 do {
 283                         goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
 284                         goaly += ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
 285                 } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
 286                 robot.move_helper.goto_notrans(goalx, goaly, NO_TURN(), &tcSlow);
 287 //              DBG_PRINT_EVENT("new survey point");
 288         }
 289         sc::result react(const evMotionError&) {
 290 //              DBG_PRINT_EVENT("can not access survey point");
 291                 return transit<survey>();
 292         }
 293         typedef mpl::list<
 294                 sc::transition<evMotionDone, survey>,
 295                 sc::custom_reaction<evMotionError> > reactions;
 296 };
 297
 298 struct go_home: TimedSimpleState< go_home, competing> {
 299         Timer t;
 300         go_home() {
 301                 //DBG_PRINT_EVENT("homing");
 302                 robot.move_helper.goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcSlow);
 303         }
 304         sc::result react(const evMotionError& ) {
 305                 //DBG_PRINT_EVENT("ERROR: home position is not reachable!");
 306                 runTimer(t, 1000, new evTimer());
 307                 return discard_event();
 308         }
 309         sc::result react (const evMotionDone&) {
 310                 //DBG_PRINT_EVENT("Mission completed!");
 311                 robot.robot_exit();
 312                 return discard_event();
 313         }
 314         typedef mpl::list<
 315                 sc::custom_reaction<evMotionError>,
 316                 sc::custom_reaction<evMotionDone>,
 317                 sc::transition<evTimer, go_home>
 318         > reactions;
 319
 320 };
 321 #endif