*/
#define NEVER
#define DEBUG
-#define FSM_MOTION
#include <sys/time.h>
#include <time.h>
#include "trgen.h"
#include "motion-control.h"
#include <hokuyo.h>
#include <ul_log.h>
+#include "timedFSM.h"
+#include <boost/mpl/list.hpp>
+#include <boost/statechart/transition.hpp>
+#include <boost/statechart/custom_reaction.hpp>
UL_LOG_CUST(ulogd_fsmmove); /* Log domain name = ulogd + name of the file */
#define DBG(format, ...)
#endif
-#define MAX_WAIT_FOR_CLOSE_MS 2000
+const int MAX_WAIT_FOR_CLOSE_MS = 2000;
// time to calculate new trajectory [seconds]
-#define TRGEN_DELAY 0.3
+const float TRGEN_DELAY = 0.3;
// Target position of the current movement
-static struct move_target current_target;
+static move_target current_target;
enum target_status {
TARGET_OK, // We can go the the target
* is moving (zero means to start movement from the current position)
* @return True on succes, false on error.
*/
-static enum target_status new_goal(struct move_target *move_target, double start_in_future)
+static target_status new_goal(struct move_target *move_target, double start_in_future)
{
- enum target_status ret;
+ target_status ret;
double angle;
PathPoint * path = NULL;
PathPoint * tmp_point = NULL;
Trajectory *t = new Trajectory(move_target->tc, backward);
if (move_target->heading.operation == TOP_ARRIVE_FROM) {
- get_arrive_from_point(move_target->x, move_target->y, move_target->heading,
- &target.x, &target.y);
+ robot.move_helper.get_arrive_from_point(move_target->x, move_target->y, move_target->heading,
+ target.x, target.y);
}
if (move_target->use_planning) {
*
* @return TARGET_OK on succes, TARGET_ERROR on error.
*/
-static enum target_status new_trajectory(Trajectory *t)
+static target_status new_trajectory(Trajectory *t)
{
Pos pos;
}
// FIXME: This is duplicite code with new_goal. Remove this
// function and use always new_goal.
- robot_get_est_pos(&pos.x, &pos.y, &pos.phi);
+ robot.get_est_pos(pos.x, pos.y, pos.phi);
pos.v = 0;
pos.omega = 0;
*
* @return
*/
-static enum target_status new_target(struct move_target *target)
+static target_status new_target(struct move_target *target)
{
- enum target_status ret;
+ target_status ret;
if (target->trajectory) {
Trajectory *t = (Trajectory*)target->trajectory;
target->tc = t->constr;
*
* @return
*/
-enum target_status
-recalculate_trajectory(void)
+target_status recalculate_trajectory(void)
{
/* TODO: Different start for planning than esitmated position */
- enum target_status ret;
+ target_status ret;
current_target.use_planning = true;
ret = new_goal(¤t_target, TRGEN_DELAY);
switch (ret) {
* The automaton
*******************************************************************************/
-FSM_STATE_DECL(movement);
-FSM_STATE_DECL(close_to_target);
-FSM_STATE_DECL(wait_for_command);
-FSM_STATE_DECL(lost);
-FSM_STATE_DECL(wait_and_try_again);
+struct movement;
+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) {}
+};
-FSM_STATE(init)
-{
- if (FSM_EVENT == EV_ENTRY) {
- FSM_TRANSITION(wait_for_command);
+struct MotionBase : sc::simple_state< MotionBase, FSMMotion, wait_for_command > {
+ sc::result react(const sc::event_base &) {
+// DBG_PRINT_EVENT("unhandled event");
+ return discard_event();
}
-}
+ typedef sc::custom_reaction< sc::event_base > reactions;
+};
-FSM_STATE(wait_for_command)
-{
- enum target_status ret;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- stop();
- break;
- case EV_NEW_TARGET:
- ret = new_target((struct move_target*)FSM_EVENT_PTR);
- switch (ret) {
- case TARGET_OK: FSM_TRANSITION(movement); break;
- case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
- case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
- }
- free(FSM_EVENT_PTR);
- break;
- case EV_TIMER:
- break;
- case EV_TRAJECTORY_DONE:
- case EV_TRAJECTORY_DONE_AND_CLOSE:
- case EV_TRAJECTORY_LOST:
- case EV_MOVE_STOP:
- case EV_OBSTACLE:
- case EV_OBSTACLE_BEHIND:
- case EV_OBSTACLE_SIDE:
- case EV_RETURN:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-FSM_STATE(movement)
-{
- static int obstacle_cntr = 0;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- break;
- case EV_TRAJECTORY_DONE:
- // Skip close to target because sometimes it turn the robot to much
-// FSM_TRANSITION(close_to_target);
-// break;
- case EV_TRAJECTORY_DONE_AND_CLOSE:
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_OBSTACLE:
- stop();
- if (obstacle_cntr++ > 5) {
- FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
- FSM_TRANSITION(wait_for_command);
- break;
- }
- switch (recalculate_trajectory()) {
- // We can go to the target:
- case TARGET_OK: break;
- // Target inaccessible because of an obstacle
- case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
- // Fatal error during planning
- case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- 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
- and we can recognize the error. */
- break;
- }
- break;
- case EV_OBSTACLE_BEHIND:
- if (robot.moves_backward && robot.use_rear_bumper)
- FSM_TRANSITION(wait_and_try_again);
- break;
- case EV_OBSTACLE_SIDE:
- if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
- (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
- FSM_TRANSITION(wait_and_try_again);
- break;
- case EV_TRAJECTORY_LOST:
- FSM_TRANSITION(lost);
- break;
- case EV_MOVE_STOP:
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_EXIT:
- break;
- case EV_NEW_TARGET:
- case EV_TIMER:
- case EV_RETURN:
- DBG_PRINT_EVENT("unhandled event");
- break;
+struct wait_for_command : TimedSimpleState< wait_for_command, MotionBase > {
+ target_status ret;
+ wait_for_command() {
+ stop();
}
-}
+ sc::result react(const evNewTarget &event) {
+ ret = new_target((move_target*)event.ev_ptr);
+ delete event.ev_ptr;
+ switch (ret) {
+ case TARGET_OK: return transit<movement>();
+ case TARGET_INACC: return transit<wait_and_try_again>();
+ case TARGET_ERROR:
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
+ ul_logerr("Target error\n");
+ }
+ return discard_event();
+ }
+ typedef sc::custom_reaction< evNewTarget > reactions;
+};
-FSM_STATE(close_to_target)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
- break;
- case EV_TRAJECTORY_DONE_AND_CLOSE:
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_OBSTACLE_BEHIND:
- if (robot.moves_backward && robot.use_rear_bumper)
- FSM_TRANSITION(wait_and_try_again);
- break;
- case EV_OBSTACLE_SIDE:
- if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
- (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
- FSM_TRANSITION(wait_and_try_again);
- break;
- case EV_TRAJECTORY_LOST:
- case EV_TIMER:
- FSM_TRANSITION(lost);
- break;
- case EV_MOVE_STOP:
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_EXIT:
- stop();
- break;
- case EV_OBSTACLE:
- case EV_RETURN:
- case EV_TRAJECTORY_DONE:
- case EV_NEW_TARGET:
- DBG_PRINT_EVENT("unhandled event");
- break;
+struct movement : TimedSimpleState< movement, MotionBase > {
+ int obstacle_cntr;
+ movement() {
+ obstacle_cntr = 0;
}
-}
+ sc::result react(const evTrajectoryDone &) {
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
+ return transit<wait_for_command>();
+ }
+ sc::result react(const evObstacle &) {
+ if (obstacle_cntr++ > 5) {
+ robot.sched.queue_event(robot.MAIN, new evMotionError());
+ return transit<wait_for_command>();
+ }
+ switch (recalculate_trajectory()) {
+ // We can go to the target:
+ case TARGET_OK: return discard_event();
+ // Target inaccessible because of an obstacle
+ case TARGET_INACC: return transit<wait_and_try_again>();
+ // Fatal error during planning
+ case TARGET_ERROR:
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
+ 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
+ and we can recognize the error. */
+ }
+ return discard_event();
+ }
+ sc::result react(const evObstacleBehind &) {
+ if (robot.moves_backward && robot.use_rear_bumper)
+ return transit<wait_and_try_again>();
+ return discard_event();
+ }
+ sc::result react(const evObstacleSide &) {
+ if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
+ (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
+ return transit<wait_and_try_again>();
+ return discard_event();
+ }
+ typedef mpl::list<
+ sc::transition< evTrajectoryLost, lost >,
+ sc::transition< evMoveStop, wait_for_command >,
+ sc::custom_reaction< evObstacleSide >,
+ sc::custom_reaction< evObstacleBehind >,
+ sc::custom_reaction< evObstacle >,
+ sc::custom_reaction< evTrajectoryDone > > reactions;
+};
-FSM_STATE(lost)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- stop();
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- switch (recalculate_trajectory()) {
- case TARGET_OK: FSM_TRANSITION(movement); break;
- case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
- case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
- }
- break;
- case EV_MOVE_STOP:
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_EXIT:
- break;
- case EV_OBSTACLE_BEHIND:
- case EV_OBSTACLE_SIDE:
- case EV_TRAJECTORY_DONE:
- case EV_NEW_TARGET:
- case EV_TRAJECTORY_DONE_AND_CLOSE:
- case EV_TRAJECTORY_LOST:
- case EV_RETURN:
- case EV_OBSTACLE:
- DBG_PRINT_EVENT("unhandled event");
+
+struct lost : TimedState< lost, MotionBase > {
+ Timer lost_tout;
+ lost(my_context ctx) : base_state(ctx) {
+ stop();
+ runTimer(lost_tout, 1000, new evTimer());
}
-}
+ sc::result react(const evTimer &) {
+ switch (recalculate_trajectory()) {
+ case TARGET_OK: return transit<movement>();
+ case TARGET_INACC: return transit<wait_and_try_again>();
+ case TARGET_ERROR:
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
+ ul_logerr("Target error\n");
+ }
+ return discard_event();
+ }
+ typedef mpl::list<
+ sc::custom_reaction< evTimer >,
+ sc::transition< evMoveStop, wait_for_command > > reactions;
+};
-FSM_STATE(wait_and_try_again)
-{
- static int target_inacc_cnt = 0;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- stop(); //FIXME: Not hard stop
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- switch (recalculate_trajectory()) {
- // We can go to the target:
- case TARGET_OK: FSM_TRANSITION(movement); break;
- // Target inaccessible because of an obstacle
- case TARGET_INACC:
- if (++target_inacc_cnt < 3) {
- FSM_TRANSITION(wait_and_try_again);
- ul_logerr("Inaccessible\n");
- FSM_TIMER(1000);
- /* FIXME (Filip): this could happen forever */
- } else { /* go away if the point is not accessible, try max. 3x */
- target_inacc_cnt = 0;
- FSM_TRANSITION(wait_for_command);
- FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
- DBG_PRINT_EVENT("Target inaccessible, go to new target.");
- }
- break;
- // Fatal error during planning
- case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
- }
- break;
- case EV_TRAJECTORY_DONE:
- case EV_TRAJECTORY_DONE_AND_CLOSE:
- FSM_TRANSITION(wait_for_command);
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- break;
- case EV_MOVE_STOP:
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_OBSTACLE_BEHIND:
- case EV_OBSTACLE_SIDE:
- break;
- case EV_NEW_TARGET:
- case EV_TRAJECTORY_LOST:
- case EV_RETURN:
- case EV_OBSTACLE:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
+struct wait_and_try_again : TimedState< wait_and_try_again, MotionBase > {
+ Timer wait_tout;
+ wait_and_try_again(my_context ctx) : base_state(ctx) {
+ stop();
+ runTimer(wait_tout, 1000, new evTimer());
}
-}
+ sc::result react(const evTimer &) {
+ switch (recalculate_trajectory()) {
+ // We can go to the target:
+ 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 */
+ // Fatal error during planning
+ case TARGET_ERROR:
+ robot.sched.queue_event(robot.MAIN, new evMotionDone());
+ ul_logerr("Target error\n");
+ }
+ return discard_event();
+ }
+ sc::result react(const evTrajectoryDone &) {
+ robot.sched.queue_event(robot.MAIN, new evTrajectoryDone());
+ return transit<wait_for_command>();
+ }
+ typedef mpl::list<
+ sc::custom_reaction< evTimer >,
+ sc::custom_reaction< evTrajectoryDone >,
+ sc::transition< evMoveStop, wait_for_command > > reactions;
+};
+
projects / eurobot / public.git / blobdiff