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
  23 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
  24
  25 /************************************************************************
  26  * Functions used in and called from all the (almost identical)
  27  * "wait for start" states in particular strategies.
  28  ************************************************************************/
  29
  30 #undef DBG_FSM_STATE
  31 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  32                                                                    fsm->debug_name, robot_current_time(), \
  33                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  34
  35 /************************************************************************
  36  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
  37  ************************************************************************/
  38
  39 struct TrajectoryConstraints tcSlow, tcVerySlow;
  40
  41
  42 void set_initial_position()
  43 {
  44         robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, DEG2RAD(HOME_POS_ANG_DEG));
  45 }
  46
  47 void actuators_home()
  48 {
  49         act_crane(CRANE_UP);
  50         act_magnet(0);
  51 }
  52
  53 /* Check if the new point is within the playground scene */
  54 bool goal_is_in_playground(double goalx, double goaly)
  55 {
  56         if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
  57                 return false;
  58         else
  59                 return true;
  60 }
  61
  62 /* Check if the new point is close to the robot */
  63 bool close_goal(double goalx, double goaly)
  64 {
  65         double x, y, phi;
  66         robot_get_est_pos(&x, &y, &phi);
  67
  68         if ((abs(goalx - x) < 0.5) && (abs(goaly - y) < 0.5) )
  69                 return true;
  70         else
  71                 return false;
  72 }
  73
  74 static bool detect_target()
  75 {
  76         // run shape detection
  77         // return TRUE when any circle detected
  78
  79         struct hokuyo_scan_type hokuyo = robot.hokuyo;
  80
  81         Shape_detect sd;
  82         std::vector<Shape_detect::Arc> arcs;
  83         sd.prepare(hokuyo.data);
  84         sd.arc_detect(arcs);
  85
  86         if (arcs.size() > 0) {
  87                 robot_pos_type e;
  88                 sharp_pos hok, target;
  89
  90                 hok.x = HOKUYO_CENTER_OFFSET_M;
  91                 hok.y = 0;
  92
  93                 robot_get_est_pos(&e.x, &e.y, &e.phi);
  94
  95                 //TODO save all detected targets and
  96                 Shape_detect::Arc * a = &arcs[0];
  97                 hok.x += (double)a->center.x / 1000.0;
  98                 hok.y = (double)a->center.y / 1000.0;
  99
 100                 /* transform target position which is relative to Hokuyo center to absolute position in space */
 101                 robot.target_pos.x = (hok.x * cos(e.phi)) - (hok.y * sin(e.phi)) + e.x;
 102                 robot.target_pos.y = (hok.x * sin(e.phi)) + (hok.y * cos(e.phi)) + e.y;
 103                 return true;
 104         } else {
 105                 return false;
 106         }
 107 }
 108
 109
 110 /* automaton to survey neighborhood in two stages */
 111 FSM_STATE(survey)
 112 {
 113         static char turn_cntr = 0;
 114         double x, y, phi;
 115
 116         switch(FSM_EVENT) {
 117                 case EV_ENTRY:
 118                         /* run shape detection
 119                         - if detection is positive go to the target
 120                         - if no target detected turn 120° to the left */
 121                         if (detect_target() & 0) {
 122                                 // target detected, go to the target
 123                                 robot.target_detected = true;
 124                                 FSM_TRANSITION(approach_target);
 125                                 DBG_PRINT_EVENT("Target detected!");
 126                         } else {
 127                                 // no target detected in this heading, turn 120°
 128                                 robot.target_detected = false;
 129                                 robot_get_est_pos(&x, &y, &phi);
 130                                 robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
 131                                 turn_cntr++;
 132                         }
 133                         break;
 134                 case EV_CRANE_DONE:
 135                         //FSM_TIMER(1000);
 136                         break;
 137                 case EV_TIMER:
 138                         if (turn_cntr > 2) {
 139                                 turn_cntr = 0;
 140                                 FSM_TRANSITION(move_around);
 141                         } else {
 142                                 FSM_TRANSITION(survey);
 143                         }
 144                         break;
 145                 case EV_MOTION_DONE:
 146                         FSM_TIMER(1000);
 147                         break;
 148                 case EV_START:
 149                 case EV_RETURN:
 150                         break;
 151                 case EV_MOTION_ERROR:
 152                 case EV_SWITCH_STRATEGY:
 153                         DBG_PRINT_EVENT("unhandled event");
 154                         FSM_TRANSITION(move_around);
 155                         break;
 156                 case EV_EXIT:
 157                         break;
 158         }
 159 }
 160
 161 FSM_STATE(approach_target)
 162 {
 163         double x, y;
 164
 165         switch(FSM_EVENT) {
 166                 case EV_ENTRY:
 167                         x = robot.target_pos.x;
 168                         y = robot.target_pos.y;
 169
 170                         if (goal_is_in_playground(x, y)) {
 171                                 /*TODO
 172                                   1. calculate position for approach
 173                                   2. approach the target to 30cm distance
 174                                   3. then use camera? to validate the target
 175                                   4. approach the target closely - using hokuyo and sharp?
 176                                   5. load the cargo
 177                                   6. move backwards 0.5m
 178                                   7. transport cargo to the home position
 179                                 */
 180                                 //ShmapSetCircleFlag(x, y, 0.1, MAP_FLAG_TARGET, 0);
 181                                 //ShmapSetCircleFlag(x, y, 0.1, MAP_FLAG_IGNORE_OBST, NULL);
 182                                 //robot_goto_notrans(x, y, NO_TURN(), &tcSlow);
 183                                 printf("regular target x:%f y:%f\n",x,y);
 184                         } else {
 185                                 DBG_PRINT_EVENT("Target out of range!");
 186                                 printf("false target x:%f y:%f\n",x,y);
 187                         }
 188                         break;
 189                 case EV_START:
 190                         break;
 191                 case EV_TIMER:
 192                         FSM_TIMER(1000);
 193                         DBG_PRINT_EVENT("Mission completed :)");
 194                         break;
 195                 case EV_RETURN:
 196                 case EV_CRANE_DONE:
 197                         break;
 198                 case EV_MOTION_DONE:
 199                         FSM_TIMER(1000);
 200                         break;
 201                 case EV_MOTION_ERROR:
 202                 case EV_SWITCH_STRATEGY:
 203                         DBG_PRINT_EVENT("unhandled event");
 204                 case EV_EXIT:
 205                         break;
 206         }
 207 }
 208
 209 FSM_STATE(move_around)
 210 {
 211         double goalx, goaly, phi;
 212         static int survey_cnt = 0;
 213
 214         switch (FSM_EVENT) {
 215                 case EV_ENTRY:
 216                         /* TODO upravit generovani nahodne pozice tak aby se lepe filtrovaly
 217                          uz prozkomane body a prekazky i body mimo hriste.
 218                          Oznacit v mape vsechny oblasti prozkoumane pomoci hokuya na kterych nedoslo k detekci??
 219                          */
 220                         do {;
 221                                 goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
 222                                 goaly += ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
 223                         } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
 224
 225                         robot_goto_notrans(goalx, goaly, NO_TURN(), &tcSlow);
 226                         survey_cnt++;
 227                         break;
 228                 case EV_MOTION_ERROR:
 229                         ul_logdeb("Goal is not reachable\n");
 230                         FSM_TIMER(1000);
 231                         break;
 232                 case EV_MOTION_DONE:
 233                         FSM_TRANSITION(survey);
 234                         break;
 235                 case EV_TIMER:
 236                         FSM_TRANSITION(move_around);
 237                         break;
 238                 case EV_RETURN:
 239                         break;
 240                 case EV_START:
 241                         /* do nothing */
 242                         break;
 243                 case EV_EXIT:
 244                         //ShmapFree();
 245                         break;
 246                 default:
 247                         break;
 248         }
 249 }
 250
 251 FSM_STATE(go_home)
 252 {
 253         switch (FSM_EVENT) {
 254                 case EV_ENTRY:
 255                         robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(0, 0.5), &tcSlow);
 256                         break;
 257                 case EV_MOTION_ERROR:
 258                         ul_logdeb("Goal is not reachable\n");
 259                         FSM_TIMER(1000);
 260                         break;
 261                 case EV_MOTION_DONE:
 262                         break;
 263                 case EV_TIMER:
 264                         FSM_TRANSITION(go_home);
 265                         break;
 266                 case EV_START:
 267                         /* do nothing */
 268                         break;
 269                 case EV_EXIT:
 270                         //ShmapFree();
 271                         break;
 272                 default:
 273                         break;
 274         }
 275 }
 276
 277 /*
 278 FSM_STATE()
 279 {
 280         switch(FSM_EVENT) {
 281                 case EV_ENTRY:
 282                         break;
 283                 case EV_START:
 284                 case EV_TIMER:
 285                 case EV_RETURN:
 286                 case EV_CRANE_DONE:
 287                 case EV_MOTION_DONE:
 288                 case EV_MOTION_ERROR:
 289                 case EV_SWITCH_STRATEGY:
 290                         DBG_PRINT_EVENT("unhandled event");
 291                 case EV_EXIT:
 292                         break;
 293         }
 294 }
 295 */