robofsm/test: Transform rectangle to C++

diff --git a/src/robofsm/test/rectangle.cc b/src/robofsm/test/rectangle.cc
index 53e55a5c1db29634f50ebc385b75893f7dd7d40f..a6670b5f645b993b6bab79cfdff0204517f74148 100644 (file)
--- a/src/robofsm/test/rectangle.cc
+++ b/src/robofsm/test/rectangle.cc
@@ -8,7 +8,7 @@
  * License: GNU GPL v.2
  */
 
-#define FSM_MAIN
+//#define FSM_MAIN
 #include <movehelper.h>
 #include <robot.h>
 #include <trgen.h>
@@ -16,26 +16,36 @@
 #include <string.h>
 #include <error.h>
 #include <ul_log.h>
+#include <events.h>
+#include <boost/statechart/asynchronous_state_machine.hpp>
+#include <boost/statechart/custom_reaction.hpp>
+#include <boost/statechart/transition.hpp>
+#include "../timedFSM.h"
+#include "../robot.h"
 
 UL_LOG_CUST(ulogd_rectangle); /* Log domain name = ulogd + name of the file */
 
-FSM_STATE_DECL(rectangle);
-FSM_STATE_DECL(turn);
-FSM_STATE_DECL(wait);
-
-FSM_STATE(init) {
-       /* Where we are at the begining? */
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       FSM_TIMER(10);
-                       break;
-               case EV_TIMER:
-                       FSM_TRANSITION(rectangle);
-                       robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
-                       break;
-               default:;
+struct rectangle;
+struct Wait;
+struct init;
+
+struct FSMMain : boost::statechart::asynchronous_state_machine<FSMMain, init, Scheduler> {
+       FSMMain(my_context ctx) : my_base(ctx) {
+               printf("%s\n", __FUNCTION__);
        }
-}
+};
+
+struct init : TimedState<init, FSMMain> {
+       Timer timeTest;
+       init(my_context ctx) : base_state(ctx) {
+               runTimer(timeTest, 10, new EV_TIMER());
+       }
+       sc::result react (const EV_TIMER &) {
+               robot.set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
+               return transit<rectangle>();
+       }
+       typedef sc::custom_reaction<EV_TIMER> reactions;
+};
 
 
 void follow_rectangle()
@@ -53,92 +63,59 @@ void follow_rectangle()
 
        /* Allocate new trajectory */
        if (!backward)
-               robot_trajectory_new(&tc);
+               robot.move_helper.trajectory_new(&tc);
        else
-               robot_trajectory_new_backward(&tc);
+               robot.move_helper.trajectory_new_backward(&tc);
 
        unsigned i, j;
        for (i=0; i<sizeof(rectangle)/sizeof(rectangle[0]); i++) {
                if (!backward) j=(i+1)%4;
                else j=(3-i);
                if (i<3) {
-                       robot_trajectory_add_point_trans(rectangle[j][0], rectangle[j][1]);
+                       robot.move_helper.add_point_trans(rectangle[j][0], rectangle[j][1]);
                } else {
-                       robot_trajectory_add_final_point_trans(rectangle[j][0], rectangle[j][1], TURN(0));
+                       robot.move_helper.add_final_point_notrans(rectangle[j][0], rectangle[j][1], TURN(0));
                }
        }
        //backward = !backward;
 }
 
-FSM_STATE(rectangle)
+struct rectangle : TimedSimpleState<rectangle, FSMMain>
 {
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       follow_rectangle();
-                       break;
-               case EV_MOTION_DONE:
-                       //FSM_TRANSITION(turn);
-                       FSM_TRANSITION(wait);
-                       break;
-               default:
-                       break;
+       rectangle() {
+               follow_rectangle();
        }
-}
+       typedef sc::transition<EV_MOTION_DONE, Wait> reactions;
+};
 
-FSM_STATE(turn)
+struct Wait : TimedState<Wait, FSMMain>
 {
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(NULL);
-                       robot_trajectory_add_final_point_trans(1, 0.5,
-                                               TURN_CCW(DEG2RAD(0)));
-                       break;
-               case EV_MOTION_DONE:
-                       FSM_TRANSITION(wait);
-                       break;
-               default:
-                       break;
+       Timer waitTout;
+       Wait(my_context ctx) : base_state(ctx) {
+               runTimer(waitTout, 1000, new EV_TIMER());
        }
-}
+       typedef sc::transition<EV_TIMER, rectangle> reactions;
+};
 
-
-FSM_STATE(wait)
-{
-       FSM_TIMER(1000);
-       switch (FSM_EVENT) {
-               case EV_TIMER:
-                       FSM_TRANSITION(rectangle);
-                       break;
-               default:
-                       break;
-       }
-}
-
-void transition_callback(struct robo_fsm *fsm)
+/*void transition_callback(struct robo_fsm *fsm)
 {
     ul_loginf("fsmcb %s: %s(%s)\n", fsm->debug_name, fsm->state_name, fsm_event_str(fsm->events[fsm->ev_head]));
-}
-
-
+}*/
 
 int main()
 {
        int rv;
 
-       rv = robot_init();
+       rv = robot.init();
        if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 
-       //robot.fsm.main.transition_callback = transition_callback;
-       //robot.fsm.motion.transition_callback = transition_callback;
-       //robot.fsm.main.debug_states = 1;
-       robot.fsm.motion.debug_states = 1;
-
-       robot.fsm.main.state = &fsm_state_main_init;
-
-        rv = robot_start();
+       //robot.fsm.main.transition_callback = transition_callback;
+       //robot.fsm.motion.transition_callback = transition_callback;
+       //robot.fsm.main.debug_states = 1;
+        rv = robot.start();
        if (rv) error(1, errno, "robot_start() returned %d\n", rv);
 
-       robot_destroy();
+       robot.destroy();
 
        return 0;
 }