robofsm: fix problems when transfered from FSM to boost statechart

[eurobot/public.git] / src / robofsm / fsmmove.cc

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index 02423be36e38e897bc050c74652c8a6a10c9ba57..96588c703535e02524f68376d0df44823e19d387 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -368,9 +368,11 @@ struct lost : TimedState< lost, MotionBase > {
 
 struct wait_and_try_again : TimedState< wait_and_try_again, MotionBase > {
        Timer wait_tout;
+       int target_inacc_cnt;
        wait_and_try_again(my_context ctx) : base_state(ctx) {
                stop();
                runTimer(wait_tout, 1000, new evTimer());
+               target_inacc_cnt = 0;
        }
        sc::result react(const evTimer &) {
                switch (recalculate_trajectory()) {
@@ -378,9 +380,15 @@ struct wait_and_try_again : TimedState< wait_and_try_again, MotionBase > {
                        case TARGET_OK: return transit<movement>();
                        // Target inaccessible because of an obstacle
                        case TARGET_INACC:
-                               ul_logerr("Inaccessible\n");
-                               return transit<wait_and_try_again>();
-                               /* FIXME (Filip): this could happen forever */ 
+                               if (++target_inacc_cnt < 3) {
+                                       ul_logerr("Inaccessible\n");
+                                       runTimer(wait_tout, 1000, new evTimer());
+                                       /* FIXME (Filip): this could happen forever */
+                               } else { /* go away if the point is not accessible, try max. 3x */
+                                       robot.sched.queue_event(robot.MAIN, new evMotionError());
+                                       //DBG_PRINT_EVENT("Target inaccessible, go to new target.");
+                                       return transit<wait_for_command>();
+                               }
                        // Fatal error during planning
                        case TARGET_ERROR: 
                                robot.sched.queue_event(robot.MAIN, new evMotionDone());