robofsm: fix problems when transfered from FSM to boost statechart

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 5f4a5830963c39ded8455a4356383d93214df41a..9d093a375893dc2d0ae8c745fe9620cddc92f72c 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -2,7 +2,6 @@
 #define COMMON_STATES_H
 
 
-#include "roboevent.h"
 #include "fsm_top.h"
 #include <robot.h>
 #include <unistd.h>
@@ -158,7 +157,31 @@ struct survey : TimedSimpleState<survey, competing> {
        Timer t;
        survey() {
                turn_cntr = 0;
-//             DBG_PRINT_EVENT("survey");
+               do_work();
+       }
+       ~survey() {
+               turn_cntr = 0;
+       }
+       sc::result react(const evTimer& ) {
+               if (turn_cntr > 2) {
+                       return transit<move_around>();
+               } else {
+                       do_work();
+                       return discard_event();
+               }
+       }
+       sc::result react(const evMotionDone&) {
+               runTimer(t,1000, new evTimer());
+               return discard_event();
+       }
+       
+       typedef mpl::list<
+               sc::custom_reaction<evTimer>,
+               sc::custom_reaction<evMotionDone>,
+               sc::transition<evMotionError, move_around>,
+               sc::transition<evEntry, approach_target> > reactions;
+      void do_work() {
+       //              DBG_PRINT_EVENT("survey");
 #if 1   // FIXME just for test
                if (detect_target()) {
 #else
@@ -181,28 +204,7 @@ struct survey : TimedSimpleState<survey, competing> {
                        turn_cntr++;
 //                     DBG_PRINT_EVENT("no target");
                }
-               
-       }
-       ~survey() {
-               turn_cntr = 0;
-       }
-       sc::result react(const evTimer& ) {
-               if (turn_cntr > 2) {
-                       return transit<move_around>();
-               } else {
-                       return transit<survey>();
-               }
-       }
-       sc::result react(const evMotionDone&) {
-               runTimer(t,1000, new evTimer());
-               return discard_event();
-       }
-       
-       typedef mpl::list<
-               sc::custom_reaction<evTimer>,
-               sc::custom_reaction<evMotionDone>,
-               sc::transition<evMotionError, move_around>,
-               sc::transition<evEntry, approach_target> > reactions;
+      }
 };
 
 /**
@@ -265,6 +267,11 @@ struct approach_target : TimedSimpleState<approach_target, competing, recognize_
                        return transit<move_around>();
                }
        }
+       typedef mpl::list<
+               sc::custom_reaction<evMotionDone>,
+               sc::custom_reaction<evReturn>,
+               sc::custom_reaction<evMotionError> > reactions;
+       
 };
 
 struct move_around : TimedSimpleState<move_around, competing> {

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());

diff --git a/src/robofsm/robot.cc b/src/robofsm/robot.cc
index 3130b03ab174780d53ae90962e2ed1c41e17846b..bb07a6be846be8cda13a4a9ffa4830682c36cb2a 100644 (file)
--- a/src/robofsm/robot.cc
+++ b/src/robofsm/robot.cc
@@ -62,19 +62,15 @@ void fill_in_known_areas_in_map()
        //ShmapSetRectangleFlag(2.74, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0); /* right */
 }
 
-static void trans_callback(struct robo_fsm *fsm)
+void trans_callback()
 {
-/*     if (fsm == &robot.fsm.main) {
-               strncpy(robot.orte.fsm_main.state_name, fsm->state_name, sizeof(robot.orte.fsm_main.state_name));
+       if (robot.MAIN == robot.sched.getActualHandle()) {
+               strncpy(robot.orte.fsm_main.state_name, robot.MAIN->state_name, sizeof(robot.orte.fsm_main.state_name));
                ORTEPublicationSend(robot.orte.publication_fsm_main);
-       } else if (fsm == &robot.fsm.motion) {
-               strncpy(robot.orte.fsm_motion.state_name, fsm->state_name, sizeof(robot.orte.fsm_motion.state_name));
+       } else if (robot.MOTION == robot.sched.getActualHandle()) {
+               strncpy(robot.orte.fsm_motion.state_name, robot.MOTION->state_name, sizeof(robot.orte.fsm_motion.state_name));
                ORTEPublicationSend(robot.orte.publication_fsm_motion);
-       } else if (fsm == &robot.fsm.act) {
-               strncpy(robot.orte.fsm_act.state_name, fsm->state_name, sizeof(robot.orte.fsm_act.state_name));
-               ORTEPublicationSend(robot.orte.publication_fsm_act);
        }
-*/
 }
 
 /**
@@ -272,4 +268,10 @@ void Robot::set_est_pos(double x, double y, double phi) {
                ref_pos.y = y;
                ref_pos.phi = phi;
        }
+}
+
+void Robot::set_state_name(const char* name)
+{
+       sched.getActualHandle()->state_name = name;
+       trans_callback();
 }
\ No newline at end of file

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index d566862a3c04b4924bd1b668408cfdbe3ac37979..f1a3fafec18ea9a5e32366416da7a8a819d2ef48 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -156,6 +156,7 @@ class Robot {
        void set_est_pos(double x, double y, double phi);
        void get_est_pos_trans(double &x, double &y, double &phi);
        void get_est_pos(double &x, double &y, double &phi);
+       void set_state_name(const char* name);
 }; /* robot */
 
 extern Robot robot;

diff --git a/src/robofsm/sub-states.h b/src/robofsm/sub-states.h
index 6eab66ccce8d6aa9992975025571eb02484327b8..ef2cb9ddf43dc735a94ffcef13d42f3780990d9c 100644 (file)
--- a/src/robofsm/sub-states.h
+++ b/src/robofsm/sub-states.h
@@ -17,7 +17,6 @@
 #include "robodata.h"
 #include "actuators.h"
 #include "guard.hpp"
-#include "roboevent.h"
 #include <timedFSM.h>
 #include <boost/mpl/list.hpp>
 #include <boost/statechart/transition.hpp>