robofsm: Start purifying old competition - remove target detection

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 32d3e6e71649dda4d7d4e9f3ef4465d84bac7ea0..d772ed1317fab91361ec0ae368f547d257b4c903 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -39,11 +39,6 @@ UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file
 
 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 
-/**
- * Vector where all absolute positions of all detected targets are stored.
- */
-std::vector<robot_pos_type> detected_target;
-
 /**
  * Safe distance for target recognition
  */
@@ -81,91 +76,6 @@ bool close_goal(double goalx, double goaly)
                 return false;
 }
 
-/**
- * Take data from hokuyo and run shape detection on it.
- *
- * Absolute positions of all detected targets centers are stored in alobal variable (vector).
- *
- * @return True if at least one target detected, else false.
- */
-static bool detect_target()
-{
-        struct hokuyo_scan_type hokuyo = robot.hokuyo;
-
-        Shape_detect sd;
-        std::vector<Shape_detect::Arc> arcs;
-        sd.prepare(hokuyo.data);
-        sd.arc_detect(arcs);
-
-        // clear old targets
-        detected_target.clear();
-
-        if (arcs.size() > 0) {
-                robot_pos_type e, target, hok;
-
-                robot_get_est_pos(&e.x, &e.y, &e.phi);
-
-                double sinus = sin(e.phi);
-                double cosinus = cos(e.phi);
-
-                // save absolute positions of all detected targets
-                for (int i = 0; i < arcs.size(); i++) {
-                        Shape_detect::Arc *a = &arcs[i];
-
-                        hok.x = HOKUYO_CENTER_OFFSET_M + (double)a->center.x / 1000.0;
-                        hok.y = (double)a->center.y / 1000.0;
-
-                        /* transform target position which is relative to Hokuyo
-                        center to absolute position in space */
-                        target.x = (hok.x * cosinus) - (hok.y * sinus) + e.x;
-                        target.y = (hok.x * sinus) + (hok.y * cosinus) + e.y;
-
-                        // filter those targets not in playground range
-                        //if (goal_is_in_playground(target.x, target.y))
-                        //        detected_target.push_back(target);
-                }
-        }
-        return detected_target.size();
-}
-
-/**
- * Calculates point to approach the target.
- *
- * @param target Position of the center of the target.
- * @param approach Pointer to the the intersection point of circle around
- * the target and line between robot center and target.
- */
-void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach)
-{
-        double xrobot, yrobot, phi;
-        double delta;
-
-        robot_get_est_pos(&xrobot, &yrobot, &phi);
-
-        delta = distance(xrobot, yrobot, xtarget, ytarget);
-
-        *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta);
-        *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta);
-
-        *phi_approach = get_approach_angle(xtarget, ytarget);
-}
-
-/**
- * Calculates point to approach the target.
- *
- * @param target Position of the center of the target.
- * @return Angle to approach the target form.
- */
-double get_approach_angle(double xtarget, double ytarget)
-{
-        double xrobot, yrobot,phi;
-
-        robot_get_est_pos(&xrobot, &yrobot, &phi);
-
-        return atan2((ytarget - yrobot), (xtarget - xrobot));
-}
-
-
 /**
  * FSM state for neighborhood observation.
  *
@@ -183,28 +93,11 @@ FSM_STATE(survey)
         switch(FSM_EVENT) {
                 case EV_ENTRY:
                         DBG_PRINT_EVENT("survey");
-#if 1   // FIXME just for test
-                        if (detect_target()) {
-#else
-                        if (turn_cntr > 1) {
-                                robot_pos_type target;
-                                detected_target.clear();
-                                for (double i = 1; i < 5; i++) {
-                                        target.x = i;
-                                        target.y = i/2.0;
-                                        detected_target.push_back(target);
-                                }
-#endif
-                                // target detected, go to the target
-                                FSM_TRANSITION(approach_target);
-                                DBG_PRINT_EVENT("Target detected!");
-                        } else {
-                                // no target detected in this heading, turn 120°
-                                robot_get_est_pos(&x, &y, &phi);
-                                robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
-                                turn_cntr++;
-                                DBG_PRINT_EVENT("no target");
-                        }
+                       // no target detected in this heading, turn 120°
+                       robot_get_est_pos(&x, &y, &phi);
+                       robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
+                       turn_cntr++;
+                       DBG_PRINT_EVENT("no target");
                         break;
                 case EV_TIMER:
                         if (turn_cntr > 2) {
@@ -225,68 +118,6 @@ FSM_STATE(survey)
         }
 }
 
-/**
- * FSM state for approaching all detected targets.
- *
- * Try to approach target.
- * If approach OK - go to subautomaton and do target recognition, touch and load.
- * On subautomaton return check if target loaded/valid.
- *
- * If target loaded, go home.
- * If target not valid, try next target if any.
- * If approach not succesfull - go to move_around state.
- */
-FSM_STATE(approach_target)
-{
-        static int target_cntr = 0;
-        int max_target = detected_target.size();
-        double x_target, y_tatget;
-        double x_approach, y_approach, phi_approach;
-
-        switch(FSM_EVENT) {
-                case EV_ENTRY:
-                        DBG_PRINT_EVENT("approaching target");
-                        x_target = detected_target[target_cntr].x;
-                        y_tatget = detected_target[target_cntr].y;
-                        target_cntr++;
-                        
-                        printf("target %d / %d\n", target_cntr, max_target);
-
-                        get_approach_point(x_target, y_tatget, &x_approach, &y_approach, &phi_approach);
-                        robot_goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.2), &tcFast);
-                        break;
-                case EV_MOTION_DONE:
-                        DBG_PRINT_EVENT("target approached");
-                        SUBFSM_TRANSITION(recognize, NULL);
-                        break;
-                case EV_RETURN:
-                        if (robot.target_loaded) {
-                                FSM_TRANSITION(go_home);
-                        } else if (robot.target_valid) {
-                                //FIXME target is valid but not loaded - try another approach direction
-
-                        } else if (!robot.target_valid && (target_cntr < max_target)) {
-                                // go for next target if any
-                                FSM_TRANSITION(approach_target);
-                        } else {
-                                // go to new point and survey
-                                FSM_TRANSITION(move_around);
-                        }
-                        break;
-                case EV_MOTION_ERROR:
-                        DBG_PRINT_EVENT("can not approach target");
-                        if (target_cntr < max_target) {
-                                FSM_TRANSITION(approach_target);
-                        } else {
-                                FSM_TRANSITION(move_around);
-                        }
-                        break;
-                case EV_EXIT:
-                        target_cntr = 0;
-                        break;
-        }
-}
-
 FSM_STATE(move_around)
 {
         double goalx, goaly;