robofsm: fix problems when transfered from FSM to boost statechart

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

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