src/robofsm/movehelper.h

   1 #ifndef MOVEHELPER_EB2008_H
   2 #define MOVEHELPER_EB2008_H
   3
   4 #include <robot.h>
   5 #include <trgenconstr.h>
   6 #include <math.h>
   7 #include <stdbool.h>
   8
   9 enum move_target_op {
  10         TOP_DONT_TURN,
  11         TOP_TURN_CW,            /* Clockwise */
  12         TOP_TURN_CCW,           /* Counter-clockwise */
  13         TOP_TURN_SHORTEST,      /* Shortest turn */
  14         TOP_ARRIVE_FROM /* Arrive to the final point from the given direction */
  15 };
  16
  17 struct move_target_heading {
  18         move_target_op operation;
  19         float angle;
  20         float distance;         /* Distance used by TOP_ARRIVE_FROM */
  21 };
  22
  23 static inline move_target_heading __target_heading(move_target_op op, float angle, float distance) {
  24         move_target_heading th;
  25         th.operation = op;
  26         th.angle = angle;
  27         th.distance = distance;
  28         return th;
  29 }
  30
  31 #define TURN(heading) __target_heading(TOP_TURN_SHORTEST, (heading), 0)
  32 #define TURN_CW(heading) __target_heading(TOP_TURN_CW, (heading), 0)
  33 #define TURN_CCW(heading) __target_heading(TOP_TURN_CCW, (heading), 0)
  34 #define NO_TURN() __target_heading(TOP_DONT_TURN, 0, 0)
  35 #define ARRIVE_FROM(heading, dist) __target_heading(TOP_ARRIVE_FROM, (heading), (dist))
  36
  37 static inline final_heading target2final_heading(move_target_heading th) {
  38         final_heading fh;
  39
  40         switch (th.operation) {
  41                 case TOP_DONT_TURN: fh.turn_type = FH_DONT_TURN; break;
  42                 case TOP_TURN_CW: fh.turn_type = FH_CW; break;
  43                 case TOP_TURN_CCW: fh.turn_type = FH_CCW; break;
  44                 case TOP_TURN_SHORTEST: fh.turn_type = FH_SHORTEST; break;
  45                 case TOP_ARRIVE_FROM: fh.turn_type = FH_DONT_TURN; break;
  46         }
  47         fh.heading = th.angle;
  48         return fh;
  49 }
  50
  51 /* Represents the target position of a move. */
  52 struct move_target {
  53         double x, y;
  54         move_target_heading heading;
  55         TrajectoryConstraints tc; /* if (trajectory) taken from it */
  56         bool use_planning; /* FIXME (FJ): this variable is not used anywhere? */
  57
  58         /** If trajectory is NULL, it is generated by motion FSM
  59          * according to the above fields. Otherwise, the trajectory
  60          * specified here is used and the above fields are used only
  61          * for replanning in case of colision. */
  62         struct Trajectory *trajectory;
  63 };
  64
  65 extern struct TrajectoryConstraints trajectoryConstraintsDefault;
  66
  67 /* extern double target_x, target_y; */
  68
  69 double trans_angle(double phi);
  70
  71 double trans_x(double x); //BLUE
  72
  73 double trans_y(double y);
  74
  75 extern bool init_ekf_flag;
  76
  77 class MoveHelper
  78 {
  79         struct Trajectory *t;
  80         move_target_heading trans_heading(move_target_heading h);
  81     public:
  82         /* Low-level trajectory handling */
  83         MoveHelper& trajectory_new(TrajectoryConstraints *tc, bool backward = false);
  84         MoveHelper& trajectory_new_backward(TrajectoryConstraints *tc) {
  85                 return trajectory_new(tc, true);
  86         }
  87         MoveHelper& add_point_notrans(double x_m, double y_m);
  88         MoveHelper& add_final_point_notrans(double x_m, double y_m, move_target_heading heading);
  89         MoveHelper& add_point_trans(double x, double y) {
  90                 return add_point_notrans(trans_x(x), trans_y(y));
  91         }
  92         MoveHelper& add_final_point_trans(double x, double y, move_target_heading heading) {
  93                 return add_final_point_notrans(trans_x(x), trans_y(y), trans_heading(heading));
  94         }
  95         void send_speed(double left, double right);
  96         void goto_notrans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc, bool planning);
  97         void move_by(double distance, move_target_heading heading, TrajectoryConstraints *tc);
  98         void stop();
  99         void goto_trans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc) {
 100                 goto_notrans(trans_x(x), trans_y(y), trans_heading(heading), tc, true);
 101         }
 102         void moveto_trans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc) {
 103                 goto_notrans(trans_x(x), trans_y(y), trans_heading(heading), tc, false);
 104         }
 105         void move_by_trans(double distance, move_target_heading heading, TrajectoryConstraints *tc) {
 106                 move_by(distance, trans_heading(heading), tc);
 107         }
 108         bool get_arrive_from_point(double target_x_m, double target_y_m, move_target_heading heading, double &point_x_m, double &point_y_m);
 109 };
 110
 111
 112 #endif /* MOVEHELPER_EB2008_H */