From f5dbf125b47a70e9ab6e8b0e3beebe2aa1ced0df Mon Sep 17 00:00:00 2001 From: Petr Silhavik Date: Wed, 20 Feb 2013 18:46:26 +0100 Subject: [PATCH] robofsm: fix problems when transfered from FSM to boost statechart --- src/robofsm/common-states.h | 55 +++++++++++++++++++++---------------- src/robofsm/fsmmove.cc | 14 ++++++++-- src/robofsm/robot.cc | 20 ++++++++------ src/robofsm/robot.h | 1 + src/robofsm/sub-states.h | 1 - 5 files changed, 54 insertions(+), 37 deletions(-) diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h index 5f4a5830..9d093a37 100644 --- 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 #include @@ -158,7 +157,31 @@ struct survey : TimedSimpleState { 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(); + } 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, + sc::custom_reaction, + sc::transition, + sc::transition > 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 { turn_cntr++; // DBG_PRINT_EVENT("no target"); } - - } - ~survey() { - turn_cntr = 0; - } - sc::result react(const evTimer& ) { - if (turn_cntr > 2) { - return transit(); - } else { - return transit(); - } - } - sc::result react(const evMotionDone&) { - runTimer(t,1000, new evTimer()); - return discard_event(); - } - - typedef mpl::list< - sc::custom_reaction, - sc::custom_reaction, - sc::transition, - sc::transition > reactions; + } }; /** @@ -265,6 +267,11 @@ struct approach_target : TimedSimpleState(); } } + typedef mpl::list< + sc::custom_reaction, + sc::custom_reaction, + sc::custom_reaction > reactions; + }; struct move_around : TimedSimpleState { diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc index 02423be3..96588c70 100644 --- 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(); // Target inaccessible because of an obstacle case TARGET_INACC: - ul_logerr("Inaccessible\n"); - return transit(); - /* 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(); + } // 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 3130b03a..bb07a6be 100644 --- 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 d566862a..f1a3fafe 100644 --- 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 6eab66cc..ef2cb9dd 100644 --- 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 #include #include -- 2.39.2