From: Michal Sojka Date: Fri, 24 Apr 2009 05:58:54 +0000 (+0200) Subject: fsmmove: Added ARRIVE_FROM target angle operation and several move helper functions X-Git-Tag: eb2009~39 X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/4487883ceef54c5cd50809d7381cb45535802e2b fsmmove: Added ARRIVE_FROM target angle operation and several move helper functions Replanning of ARRIVE_FROM needs to be fixed. --- diff --git a/src/motion/trgen.h b/src/motion/trgen.h index 7dceecd3..714b3d8b 100644 --- a/src/motion/trgen.h +++ b/src/motion/trgen.h @@ -254,7 +254,7 @@ public: Trajectory(TrajectoryConstraints tc, bool _backward = false) : wayPoints(), initialPoint(0), finalPoint(0), prepared(false), constr(tc), backward(_backward) { - finalHeading = *NO_TURN(); + finalHeading.turn_type = FH_DONT_TURN; }; ~Trajectory() { diff --git a/src/motion/trgenconstr.h b/src/motion/trgenconstr.h index 7eed37ea..3408ffff 100644 --- a/src/motion/trgenconstr.h +++ b/src/motion/trgenconstr.h @@ -18,26 +18,12 @@ enum turn_type { FH_DONT_TURN, FH_CW, /* Clockwise */ FH_CCW, /* Counter-clockwise */ - FH_SHORTEST /* Shortest turn */ + FH_SHORTEST, /* Shortest turn */ }; struct final_heading { enum turn_type turn_type; - double heading; /* Heading in rad */ + float heading; /* Heading in rad */ }; -static inline struct final_heading *__turn(enum turn_type tt, double heading) -{ - static struct final_heading fh; - fh.turn_type = tt; - fh.heading = heading; - return &fh; -} - -/* FIXME: Use GCC extension for this */ -#define TURN(heading) __turn(FH_SHORTEST, (heading)) -#define TURN_CW(heading) __turn(FH_CW, (heading)) -#define TURN_CCW(heading) __turn(FH_CCW, (heading)) -#define NO_TURN() __turn(FH_DONT_TURN, 0) - #endif diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc index 40da3afe..b6d39726 100644 --- a/src/robofsm/fsmmove.cc +++ b/src/robofsm/fsmmove.cc @@ -53,57 +53,58 @@ enum target_status { static enum target_status new_goal(struct move_target *target, double start_in_future) { enum target_status ret; - double startx, starty, angle; + double angle, targetx, targety; PathPoint * path = NULL; PathPoint * tmp_point = NULL; - - struct TrajectoryConstraints tc; Pos start; - //tc.maxv *= 2.5; - //tc.maxacc *= 2.5; - double time_ts; + bool backward = false; // Where to start trajectory planning? get_future_pos(start_in_future, start, time_ts); + Trajectory *t = new Trajectory(target->tc, backward); - tc = target->tc; - - //DBG("Going from (%.2f, %.2f) to (%.2f, %.2f)\n", startx, starty, goalx, goaly); - /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1. - startx = start.x; - starty = start.y; - switch (path_planner(startx, starty, target->x, target->y, &path, &angle)) { - case PP_ERROR_MAP_NOT_INIT: - ret = TARGET_ERROR; break; - case PP_ERROR_GOAL_IS_OBSTACLE: - case PP_ERROR_GOAL_NOT_REACHABLE: - ret = TARGET_INACC; break; - default: - ret = TARGET_OK; break; + if (target->heading.operation == TOP_ARRIVE_FROM) { + get_arrive_from_point(target->x, target->y, target->heading, + &targetx, &targety); + } else { + targetx = target->x; + targety = target->y; } - if (ret != TARGET_OK) - { - return ret; - } - - bool backward = false; - Trajectory *t = new Trajectory(tc, backward); + if (target->use_planning) { + /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1. + switch (path_planner(start.x, start.y, target->x, target->y, &path, &angle)) { + case PP_ERROR_MAP_NOT_INIT: + ret = TARGET_ERROR; break; + case PP_ERROR_GOAL_IS_OBSTACLE: + case PP_ERROR_GOAL_NOT_REACHABLE: + ret = TARGET_INACC; break; + default: + ret = TARGET_OK; break; + } - // Add this path to the trajectory. - for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) { - DBG("ADDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y); - t->addPoint(tmp_point->x, tmp_point->y); + if (ret != TARGET_OK) + return ret; + // Add this path to the trajectory. + for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) { + DBG("ADDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y); + t->addPoint(tmp_point->x, tmp_point->y); + } + freePathMemory(path); + } else { + t->addPoint(targetx, targety); } - t->finalHeading = target->h; - freePathMemory(path); + if (target->heading.operation == TOP_ARRIVE_FROM) { + t->addPoint(target->x, target->y); + } + t->finalHeading = target2final_heading(target->heading); if (t->prepare(start)) { if (start_in_future) { /* not to switch immediately - switch_trajectory_at(t, time_ts); */ + switch_trajectory_at(t, time_ts); */ // new test - appending trajectories go(t, time_ts); } else { @@ -154,15 +155,17 @@ static enum target_status new_target(struct move_target *target) enum target_status ret; if (target->trajectory) { Trajectory *t = (Trajectory*)target->trajectory; - ret = new_trajectory(t); - // Trajectory is deleted by somebody else target->tc = t->constr; + ret = new_trajectory(t); target->trajectory = NULL; + // Trajectory is deleted by somebody else } else { ret = new_goal(target, 0); } if (ret != TARGET_ERROR) { current_target = *target; + // On recaclulation we always use path planner + current_target.use_planning = true; } return ret; } diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc index a7648662..029bb0cd 100644 --- a/src/robofsm/movehelper.cc +++ b/src/robofsm/movehelper.cc @@ -82,14 +82,7 @@ void robot_set_est_pos_notrans(double x, double y, double phi) if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M; if (y<0) y=0; if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M; - struct mcl_robot_pos *newpos = (struct mcl_robot_pos*)malloc(sizeof(struct mcl_robot_pos)); - memset(newpos, 0, sizeof(*newpos)); - newpos->x = x; - newpos->y = y; - newpos->angle = fmod(phi, 2.0*M_PI); - // FIXME: This should be synchronous signal - we need to - // define FSM_SIGNAL_SYNC - //FSM_SIGNAL(LOC, EV_SET_ESTIMATED, newpos); + // TODO: initialize ekf here ROBOT_LOCK(est_pos); robot.est_pos.x = x; robot.est_pos.y = y; @@ -113,6 +106,23 @@ void robot_trajectory_add_point_notrans(double x_m, double y_m) } } +bool get_arrive_from_point(double target_x_m, double target_y_m, struct move_target_heading heading, + double *point_x_m, double *point_y_m) +{ + double x, y; + if (heading.operation == TOP_ARRIVE_FROM) { + x = target_x_m + + heading.distance*cos(heading.angle-M_PI); + y = target_y_m + + heading.distance*sin(heading.angle-M_PI); + *point_x_m = x; + *point_y_m = y; + return true; + } else { + return false; + } +} + /** * Adds final point to trajectory objects and starts robot movement. * @@ -125,21 +135,28 @@ void robot_trajectory_add_point_notrans(double x_m, double y_m) * counter-clockwise and DEG2RAD(-270) means turn up * clockwise. */ -void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct final_heading *heading) +void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct move_target_heading heading) { struct move_target *target; - robot_trajectory_add_point_notrans(x_m, y_m); Trajectory *t; t = (Trajectory*)robot.new_trajectory; if (t) { - t->finalHeading = *heading; + if (heading.operation == TOP_ARRIVE_FROM) { + double ax, ay; + get_arrive_from_point(x_m, y_m, heading, &ax, &ay); + robot_trajectory_add_point_notrans(ax, ay); + } + robot_trajectory_add_point_notrans(x_m, y_m); + + t->finalHeading = target2final_heading(heading); target = (struct move_target*)malloc(sizeof(*target)); memset(target, 0, sizeof(*target)); target->x = x_m; target->y = y_m; - target->h = *heading; + target->heading = heading; target->trajectory = t; + target->use_planning = false; FSM_SIGNAL(MOTION, EV_NEW_TARGET, target); robot.new_trajectory = NULL; } else { @@ -186,20 +203,37 @@ void robot_send_speed(double left, double right) { * @param y Y coordinate in meters. * @param heading Desired heading of the robot at goal point. */ -void robot_goto_notrans(double x, double y, struct final_heading *heading, struct TrajectoryConstraints *tc) +void robot_goto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc) { struct move_target *target; target = (struct move_target*)malloc(sizeof(*target)); memset(target, 0, sizeof(*target)); target->x = x; target->y = y; - target->h = *heading; + target->heading = heading; + target->use_planning = true; if (tc) target->tc = *tc; else target->tc = trajectoryConstraintsDefault; FSM_SIGNAL(MOTION, EV_NEW_TARGET, target); } +void robot_moveto_notrans(double x, double y, + struct move_target_heading heading, + struct TrajectoryConstraints *tc) +{ + struct move_target *target; + target = (struct move_target*)malloc(sizeof(*target)); + memset(target, 0, sizeof(*target)); + target->x = x; + target->y = y; + target->heading = heading; + if (tc) target->tc = *tc; + else target->tc = trajectoryConstraintsDefault; + target->use_planning = false; + + FSM_SIGNAL(MOTION, EV_NEW_TARGET, target); +} /** * Move robot forward or backward @@ -208,7 +242,7 @@ void robot_goto_notrans(double x, double y, struct final_heading *heading, struc * @param heading Final heading * @param tc Trajectory constrains */ -void robot_move_by(double distance, struct final_heading *heading, struct TrajectoryConstraints *tc) +void robot_move_by(double distance, struct move_target_heading heading, struct TrajectoryConstraints *tc) { double x, y, phi; @@ -226,14 +260,16 @@ void robot_move_by(double distance, struct final_heading *heading, struct Trajec bool backward = distance < 0; t = new Trajectory(*tc, backward); t->addPoint(x, y); - t->finalHeading = *heading; + t->finalHeading = target2final_heading(heading); struct move_target *target; target = (struct move_target*)malloc(sizeof(*target)); memset(target, 0, sizeof(*target)); target->x = x; target->y = y; - target->h = *heading; + target->heading = heading; target->trajectory = t; FSM_SIGNAL(MOTION, EV_NEW_TARGET, target); } + + diff --git a/src/robofsm/movehelper.h b/src/robofsm/movehelper.h index 3dd2eba3..1c0ca3dd 100644 --- a/src/robofsm/movehelper.h +++ b/src/robofsm/movehelper.h @@ -5,14 +5,64 @@ #include #include #include +#include + +enum move_target_op { + TOP_DONT_TURN, + TOP_TURN_CW, /* Clockwise */ + TOP_TURN_CCW, /* Counter-clockwise */ + TOP_TURN_SHORTEST, /* Shortest turn */ + TOP_ARRIVE_FROM /* Arrive to the final point from the given direction */ +}; + +struct move_target_heading { + enum move_target_op operation; + float angle; + float distance; /* Distance used by TOP_ARRIVE_FROM */ +}; + +static inline struct move_target_heading __target_heading(enum move_target_op op, float angle, float distance) +{ + struct move_target_heading th; + th.operation = op; + th.angle = angle; + th.distance = distance; + return th; +} + +#define TURN(heading) __target_heading(TOP_TURN_SHORTEST, (heading), 0) +#define TURN_CW(heading) __target_heading(TOP_TURN_CW, (heading), 0) +#define TURN_CCW(heading) __target_heading(TOP_TURN_CCW, (heading), 0) +#define NO_TURN() __target_heading(TOP_DONT_TURN, 0, 0) +#define ARRIVE_FROM(heading, dist) __target_heading(TOP_ARRIVE_FROM, (heading), (dist)) -/* Represents the target position of a move. If the trajectory is not - * specified, it is created by path planning algorithm. */ +static inline struct final_heading target2final_heading(struct move_target_heading th) +{ + struct final_heading fh; + + switch (th.operation) { + case TOP_DONT_TURN: fh.turn_type = FH_DONT_TURN; break; + case TOP_TURN_CW: fh.turn_type = FH_CW; break; + case TOP_TURN_CCW: fh.turn_type = FH_CCW; break; + case TOP_TURN_SHORTEST: fh.turn_type = FH_SHORTEST; break; + case TOP_ARRIVE_FROM: fh.turn_type = FH_DONT_TURN; break; + } + fh.heading = th.angle; + return fh; +} + +/* Represents the target position of a move. */ struct move_target { double x, y; - struct final_heading h; - void *trajectory; + struct move_target_heading heading; struct TrajectoryConstraints tc; /* if (trajectory) taken from it */ + bool use_planning; + + /** If trajectory is NULL, it is generated by motion FSM + * according to the above fields. Otherwise, the trajectory + * specified here is used and the above fields are used only + * for replanning in case of colision. */ + void *trajectory; }; extern struct TrajectoryConstraints trajectoryConstraintsDefault; @@ -41,7 +91,7 @@ static inline double __trans_ang(double phi) if (phi < 0 && a < 0) a += 2.0*M_PI; return a; } -static inline struct final_heading* __trans_heading(struct final_heading *h) +static inline struct final_heading __trans_heading(struct sd h) { if (h->turn_type != FH_DONT_TURN) { h->heading = __trans_ang(h->heading); @@ -72,28 +122,41 @@ static inline struct final_heading* __trans_heading(struct final_heading *h) #define TRANS_HEADING(h) (h) #endif +bool get_arrive_from_point(double target_x_m, double target_y_m, + struct move_target_heading heading, + double *point_x_m, double *point_y_m); void robot_set_est_pos_notrans(double x, double y, double phi); #define robot_set_est_pos_trans(x, y, phi) robot_set_est_pos_notrans(TRANS_X(x), TRANS_Y(y), TRANS_ANG(phi)) +void robot_send_speed(double left, double right); + +/* Low-level trajectory handling */ void robot_trajectory_new(struct TrajectoryConstraints *tc); void robot_trajectory_new_backward(struct TrajectoryConstraints *tc); void robot_trajectory_add_point_notrans(double x_m, double y_m); #define robot_trajectory_add_point_trans(x, y) robot_trajectory_add_point_notrans(TRANS_X(x), TRANS_Y(y)) -void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct final_heading *heading); +void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct move_target_heading heading); #define robot_trajectory_add_final_point_trans(x, y, heading) robot_trajectory_add_final_point_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading)) +/** Stop robot immediately */ void robot_stop(); -void robot_send_speed(double left, double right); - -void robot_goto_notrans(double x, double y, struct final_heading *heading, struct TrajectoryConstraints *tc); +/** Go to a point using path planning to avoid apriori known obstacles */ +void robot_goto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc); #define robot_goto_trans(x, y, heading, tc) robot_goto_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading), tc) -void robot_move_by(double distance, struct final_heading *heading, struct TrajectoryConstraints *tc); +/** Move to a point using straight line trajectory. If ARIVE_FROM is + * set, then the trajectory will be composed two lines.... TODO */ +void robot_moveto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc); +#define robot_moveto_trans(x, y, heading, tc) robot_goto_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading), tc) + +/* Relative movement */ +void robot_move_by(double distance, struct move_target_heading heading, struct TrajectoryConstraints *tc); +#define robot_move_by_trans(distance, heading, tc) robot_move_by(distance, TRANS_HEADING(heading), tc) /* FIXME: not used anywhere */ static inline void robot_translate_coordinates(double *x, double *y, double *phi)