robofsm: Remove state close_to_target

This was done because this state was inaccessible.

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index 3459fcbb05cc17f4fb674d874d8e53aa92abe699..95e1256e124fc7f4332e5a3abb87b5c7ba319676 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -273,16 +273,14 @@ recalculate_trajectory(void)
  *******************************************************************************/
 
 struct movement;
-struct close_to_target;
+//struct close_to_target;
 struct wait_for_command;
 struct lost;
 struct wait_and_try_again;
 struct MotionBase;
 
 struct FSMMotion : boost::statechart::asynchronous_state_machine<FSMMotion, MotionBase, Scheduler> {
-       FSMMotion(my_context ctx) : my_base(ctx) {
-               printf("%s\n", __FUNCTION__);
-       }
+       FSMMotion(my_context ctx) : my_base(ctx) {}
 };
 
 struct MotionBase : sc::simple_state<MotionBase, FSMMotion, wait_for_command> {
@@ -338,18 +336,16 @@ struct wait_for_command : TimedSimpleState<wait_for_command, MotionBase>
 {
        enum target_status ret;
        wait_for_command() {
-               printf("%s\n", __FUNCTION__);
                stop();
        }
        sc::result react(const EV_NEW_TARGET &event) {
-               printf("%s EV_NEW_TARGET\n", __FUNCTION__);
                ret = new_target((struct move_target*)event.ev_ptr);
                free (event.ev_ptr);
                switch (ret) {
                        case TARGET_OK: return transit<movement>(); 
                        case TARGET_INACC: return transit<wait_and_try_again>();
                        case TARGET_ERROR: 
-                         /*FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL)*/; 
+                         robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
                          ul_logerr("Target error\n"); 
                }
                return discard_event();
@@ -363,13 +359,10 @@ struct movement : TimedSimpleState<movement, MotionBase>
                // Skip close to target because sometimes it turn the robot to much
                // FSM_TRANSITION(close_to_target);
                robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
-               /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
                return transit<wait_for_command>();
        }
        sc::result react(const EV_TRAJECTORY_DONE_AND_CLOSE &) {
-               
                robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
-               /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
                return transit<wait_for_command>();
        }
        sc::result react(const EV_OBSTACLE &) {
@@ -380,7 +373,7 @@ struct movement : TimedSimpleState<movement, MotionBase>
                        case TARGET_INACC: return transit<wait_and_try_again>(); 
                        // Fatal error during planning
                        case TARGET_ERROR: 
-                         /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
+                         robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
                           ul_logerr("Target error\n");
                           /* FIXME (Filip): shouldn't transition to wait_for_command be here?
                             No, probably not, if an error occurs, robot won't move any more
@@ -412,7 +405,7 @@ struct movement : TimedSimpleState<movement, MotionBase>
                sc::custom_reaction<EV_TRAJECTORY_DONE>,
                sc::custom_reaction<EV_TRAJECTORY_DONE_AND_CLOSE> > reactions;
 };
-
+/*
 struct close_to_target : TimedState<close_to_target, MotionBase>{
        Timer timeout_for_close;
        close_to_target(my_context ctx) : base_state(ctx) {
@@ -422,7 +415,7 @@ struct close_to_target : TimedState<close_to_target, MotionBase>{
                stop();
        }
        sc::result react(const EV_TRAJECTORY_DONE_AND_CLOSE &) {
-               /*TODO  FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
+               robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
                return transit<wait_for_command>();
        }
        sc::result react(const EV_OBSTACLE_BEHIND &) {
@@ -448,7 +441,7 @@ struct close_to_target : TimedState<close_to_target, MotionBase>{
                sc::transition<EV_TIMER, lost>,
                sc::transition<EV_MOVE_STOP, wait_for_command> > reactions;
 };
-
+*/
 struct lost : TimedState<lost, MotionBase>{
        Timer lost_tout;
        lost(my_context ctx) : base_state(ctx) {
@@ -460,7 +453,7 @@ struct lost : TimedState<lost, MotionBase>{
                        case TARGET_OK:    return transit<movement>();
                        case TARGET_INACC: return transit<wait_and_try_again>();
                        case TARGET_ERROR: 
-                           /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/ 
+                           robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
                            ul_logerr("Target error\n"); 
                }
                return discard_event();
@@ -487,17 +480,17 @@ struct wait_and_try_again : TimedState<wait_and_try_again, MotionBase> {
                                /* FIXME (Filip): this could happen forever */ 
                        // Fatal error during planning
                        case TARGET_ERROR: 
-                               /*TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/ 
+                               robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
                                ul_logerr("Target error\n");
                }
                return discard_event();
        }
        sc::result react(const EV_TRAJECTORY_DONE &) {
-               /* TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
+               robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
                return transit<wait_for_command>();
        }
        sc::result react(const EV_TRAJECTORY_DONE_AND_CLOSE &) {
-               /* TODO FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);*/
+               robot.sched.queue_event(robot.MAIN, new EV_MOTION_DONE());
                return transit<wait_for_command>();
        }
        typedef mpl::list<