robofsm: Cleanup - remove unused code + small bear detection tuning

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 082fa34ad76f59bc4213e64baa4d47f59f7d7f45..2496142b02ddad174007e9da3cbda441dfc9fa29 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -84,53 +84,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.
  *
@@ -162,66 +115,6 @@ double get_approach_angle(double xtarget, double ytarget)
         return atan2((ytarget - ycenter), (xtarget - xcenter));
 }
 
-
-/**
- * FSM state for neighborhood observation.
- *
- * Detect targets using shape_detect.
- * If no target detected, turn 120deg and try again.
- * Scan all 360deg and then go back to move_around state.
- *
- * If target detected, go to approach_target state.
- */
-FSM_STATE(survey)
-{
-        static char turn_cntr = 0;
-        double x, y, phi;
-
-        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");
-                        }
-                        break;
-                case EV_TIMER:
-                        if (turn_cntr > 2) {
-                                FSM_TRANSITION(move_around);
-                        } else {
-                                FSM_TRANSITION(survey);
-                        }
-                        break;
-                case EV_MOTION_DONE:
-                        FSM_TIMER(1000);
-                        break;
-                case EV_MOTION_ERROR:
-                        FSM_TRANSITION(move_around);
-                        break;
-                case EV_EXIT:
-                        turn_cntr = 0;
-                        break;
-        }
-}
-
 /**
  * FSM state for approaching all detected targets.
  *
@@ -239,7 +132,6 @@ FSM_STATE(approach_target)
 
         switch(FSM_EVENT) {
                 case EV_ENTRY:
-                        enable_obstacle_avoidance(false);
                         DBG_PRINT_EVENT("approaching target");
 
                         get_approach_point(robot.target_pos.x, robot.target_pos.y, &x_approach, &y_approach, &phi_approach);
@@ -287,6 +179,7 @@ FSM_STATE(move_around)
                 case EV_TARGET_DETECTED:
                         robot_stop();
                         robot.target_localization_enab = false;
+                        enable_obstacle_avoidance(false);
                         FSM_TRANSITION(approach_target);
                         break;
                 case EV_TIMER:
@@ -313,11 +206,6 @@ FSM_STATE(approach_arena)
                         act_jaws(OPEN);
                         FSM_TIMER(1000);
                         break;
-                case EV_TARGET_DETECTED:
-                        robot_stop();
-                        robot.target_localization_enab = false;
-                        FSM_TRANSITION(approach_target);
-                        break;
                 case EV_TIMER:
                         enable_obstacle_avoidance(true);
                         robot.target_localization_enab = true;
@@ -347,23 +235,3 @@ FSM_STATE(go_home)
                         break;
         }
 }
-
-/*
-FSM_STATE()
-{
-        switch(FSM_EVENT) {
-                case EV_ENTRY:
-                        break;
-                case EV_START:
-                case EV_TIMER:
-                case EV_RETURN:
-                case EV_CRANE_DONE:
-                case EV_MOTION_DONE:
-                case EV_MOTION_ERROR:
-                case EV_SWITCH_STRATEGY:
-                        DBG_PRINT_EVENT("unhandled event");
-                case EV_EXIT:
-                        break;
-        }
-}
-*/

diff --git a/src/robofsm/sub-states.cc b/src/robofsm/sub-states.cc
index 75f085cbd412f97acfa1cb52707cc363e2fe5d73..07e1b5c38c2275ac3757ca9a2ea09eed19b32e94 100644 (file)
--- a/src/robofsm/sub-states.cc
+++ b/src/robofsm/sub-states.cc
@@ -89,7 +89,6 @@ FSM_STATE(get_target_turn)
 
         switch(FSM_EVENT) {
                 case EV_ENTRY:
-                        enable_obstacle_avoidance(false);
                         DBG_PRINT_EVENT("turn");
                         get_hokuyo_min(&alpha, &minimum);
 
@@ -155,32 +154,6 @@ FSM_STATE(get_target_load)
         }
 }
 
-/* unload target if not valid = no color match using OMRON */
-FSM_STATE(get_target_unload)
-{
-        static int direction = 0;
-
-        switch(FSM_EVENT) {
-                case EV_ENTRY:
-                        direction = 0;
-                        act_jaws(OPEN);
-                        break;
-                case EV_TIMER:
-                        act_jaws(CATCH);
-                        robot.target_loaded = true;
-                        FSM_TRANSITION(get_target_back);
-                        break;
-                case EV_JAWS_DONE:
-                        if (direction == 0) {
-                                direction = 1;
-                                FSM_TIMER(2000);
-                        }
-                        break;
-                case EV_EXIT:
-                        break;
-        }
-}
-
 FSM_STATE(get_target_back)
 {
         switch(FSM_EVENT) {
@@ -192,30 +165,11 @@ FSM_STATE(get_target_back)
                 case EV_JAWS_DONE:
                         break;
                 case EV_MOTION_DONE:
-                case EV_MOTION_ERROR:
                         SUBFSM_RET(NULL);
                         break;
-                case EV_EXIT:
-                        break;
-        }
-}
-
-/*
-FSM_STATE()
-{
-        switch(FSM_EVENT) {
-                case EV_ENTRY:
-                        break;
-                case EV_START:
-                case EV_TIMER:
-                case EV_RETURN:
-                case EV_CRANE_DONE:
-                case EV_MOTION_DONE:
                 case EV_MOTION_ERROR:
-                case EV_SWITCH_STRATEGY:
                         DBG_PRINT_EVENT("unhandled event");
                 case EV_EXIT:
                         break;
         }
 }
-*/
\ No newline at end of file