#include <mcl_robot.h>
#include <string.h>
#include <ul_log.h>
+#include "guard.hpp"
+#include "events.h"
UL_LOG_CUST(ulogd_movehelper); /* Log domain name = ulogd + name of the file */
#if 0
-struct TrajectoryConstraints trajectoryConstraintsDefault = {
+TrajectoryConstraints trajectoryConstraintsDefault = {
maxv: 0.3, // m/s
maxomega: 1.5, // rad/s
maxangacc: 2, // rad/s^2
maxe: (double)ROBOT_WIDTH_MM/2.0/1000.0 // m
};
#else
-struct TrajectoryConstraints trajectoryConstraintsDefault = {
+TrajectoryConstraints trajectoryConstraintsDefault = {
maxv: 1, // m/s
maxomega: 1, // rad/s
maxangacc: 1, // rad/s^2
};
#endif
-/**
- *
- *
- *
- */
-static void robot_trajectory_new_ex(struct TrajectoryConstraints *tc, bool backward)
-{
- Trajectory *t;
-
- if (!tc) tc = &trajectoryConstraintsDefault;
-
- t = (Trajectory*)robot.new_trajectory;
- if (t) delete(t);
- t = new Trajectory(*tc, backward);
- robot.new_trajectory = t;
-}
-
/**
* Initializes new trajectory object for adding points. Path planner
* will not be used.
*
* @param tc Constrains for the trajectory.
+ * @param backward Backward trajectory, implicit value false.
*/
-void robot_trajectory_new(struct TrajectoryConstraints *tc) {
- robot_trajectory_new_ex(tc, false);
-}
-void robot_trajectory_new_backward(struct TrajectoryConstraints *tc) {
- robot_trajectory_new_ex(tc, true);
+MoveHelper& MoveHelper::trajectory_new(TrajectoryConstraints* tc, bool backward) {
+ if (!tc) tc = &trajectoryConstraintsDefault;
+ if (t) delete t;
+ t = new Trajectory(*tc, backward);
+ return *this;
}
-bool init_ekf_flag = false;
-
-/** Sets actual position of the robot and with respoect to color of
- * the team. Should be used for setting initial position of the
- * robot. */
-void robot_set_est_pos_notrans(double x, double y, double phi)
-{
- if (x<0) x=0;
- if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
- if (y<0) y=0;
- if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
-
- ROBOT_LOCK(est_pos_indep_odo);
- robot.est_pos_indep_odo.x = x;
- robot.est_pos_indep_odo.y = y;
- robot.est_pos_indep_odo.phi = phi;
- ROBOT_UNLOCK(est_pos_indep_odo);
-
- ROBOT_LOCK(est_pos_odo);
- robot.est_pos_odo.x = x;
- robot.est_pos_odo.y = y;
- robot.est_pos_odo.phi = phi;
- ROBOT_UNLOCK(est_pos_odo);
-
- ROBOT_LOCK(ref_pos);
- robot.ref_pos.x = x;
- robot.ref_pos.y = y;
- robot.ref_pos.phi = phi;
- ROBOT_UNLOCK(ref_pos);
-}
/**
* Adds point in absolute coordinates to previously initialized trajectory object.
* @param x_m X coordinate in meters.
* @param y_m Y coordinate in meters.
*/
-void robot_trajectory_add_point_notrans(double x_m, double y_m)
-{
- Trajectory *t;
-
- t = (Trajectory*)robot.new_trajectory;
+MoveHelper& MoveHelper::add_point_notrans(double x_m, double y_m) {
if (t) {
t->addPoint(x_m, y_m);
}
+ return *this;
}
-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.
* counter-clockwise and DEG2RAD(-270) means turn up
* clockwise.
*/
-void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct move_target_heading heading)
-{
- struct move_target *target;
- Trajectory *t;
-
- t = (Trajectory*)robot.new_trajectory;
+MoveHelper& MoveHelper::add_final_point_notrans(double x_m, double y_m, move_target_heading heading) {
+ move_target *target;
if (t) {
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);
+ get_arrive_from_point(x_m, y_m, heading, ax, ay);
+ add_point_notrans(ax, ay);
}
- robot_trajectory_add_point_notrans(x_m, y_m);
+ 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 = new move_target();
target->x = x_m;
target->y = y_m;
target->heading = heading;
target->trajectory = t;
target->use_planning = false;
- FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
- robot.new_trajectory = NULL;
+ robot.sched.queue_event(robot.MOTION, new evNewTarget(target));
+ t = NULL;
} else {
ul_logerr("%s: Error - No trajectory\n", __FUNCTION__);
}
+ return *this;
+}
+
+bool MoveHelper::get_arrive_from_point(double target_x_m, double target_y_m, 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;
+ }
}
+
/**
* Stops actual movement.
*
*/
-void robot_stop()
-{
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- robot.orte.motion_speed.left = 0xFFFF;
- robot.orte.motion_speed.right = 0xFFFF;
- ORTEPublicationSend(robot.orte.publication_motion_speed);
+void MoveHelper::stop() {
+ robot.sched.queue_event(robot.MOTION, new evMoveStop());
}
-void robot_send_speed(double left, double right) {
+void MoveHelper::send_speed(double left, double right) {
double mul;
unsigned l, r;
*
* @param x X coordinate in meters.
* @param y Y coordinate in meters.
- * @param heading Desired heading of the robot at goal point.
+ * @param heading Desired heading of the robot at goal point.
+ * @param planning Use planning or not.
*/
-void robot_goto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc)
-{
- /* FIXME (FJ) : robot_goto_notrans and robot_moveto_notrans are almost identical,
- * join the code into one function and call it from both of them */
- struct move_target *target;
- target = (struct move_target*)malloc(sizeof(*target));
- memset(target, 0, sizeof(*target));
+void MoveHelper::goto_notrans(double x, double y, move_target_heading heading, TrajectoryConstraints* tc, bool planning) {
+ move_target *target = new move_target();
target->x = x;
target->y = y;
target->heading = heading;
- target->use_planning = true; /* FIXME: this is not used anywhere? (FJ) */
+ target->use_planning = planning;
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);
+ robot.sched.queue_event(robot.MOTION, new evNewTarget(target));
}
/**
* @param heading Final heading
* @param tc Trajectory constrains
*/
-void robot_move_by(double distance, struct move_target_heading heading, struct TrajectoryConstraints *tc)
-{
+void MoveHelper::move_by(double distance, move_target_heading heading, TrajectoryConstraints* tc) {
double x, y, phi;
- robot_get_est_pos(&x, &y, &phi);
+ robot.get_est_pos(x, y, phi);
x += distance*cos(phi);
y += distance*sin(phi);
- Trajectory *t;
if (!tc) tc = &trajectoryConstraintsDefault;
bool backward = distance < 0;
t = new Trajectory(*tc, backward);
t->addPoint(x, y);
t->finalHeading = target2final_heading(heading);
- struct move_target *target;
- target = (struct move_target*)malloc(sizeof(*target));
- memset(target, 0, sizeof(*target));
+ move_target *target = new move_target();
target->x = x;
target->y = y;
target->heading = heading;
target->trajectory = t;
- FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
+ robot.sched.queue_event(robot.MOTION, new evNewTarget(target));
+ delete t;
}
+move_target_heading MoveHelper::trans_heading(move_target_heading h) {
+ if (robot.team_color == BLUE) {
+ return h;
+ } else {
+ if (h.operation != TOP_DONT_TURN) {
+ h.angle = trans_angle(h.angle);
+ switch (h.operation) {
+ case FH_CW:
+ h.operation = TOP_TURN_CCW;
+ break;
+ case FH_CCW:
+ h.operation = TOP_TURN_CW;
+ break;
+ default:
+ break;
+ }
+ }
+ return h;
+ }
+}
+
+double trans_angle(double phi) {
+ if (robot.team_color == BLUE) {
+ return phi;
+ } else {
+ double a;
+ a = M_PI/2.0 - phi + M_PI/2.0;
+ a = fmod(a, 2.0*M_PI);
+ if (phi >= 0 && a >= 0) a -= 2.0*M_PI;
+ if (phi < 0 && a < 0) a += 2.0*M_PI;
+ return a;
+ }
+}
+
+double trans_x(double x) {
+ if (robot.team_color == BLUE)
+ return x;
+ else
+ return PLAYGROUND_WIDTH_M - x;
+}
+double trans_y(double y) {
+ return y;
+}
\ No newline at end of file
};
struct move_target_heading {
- enum move_target_op operation;
+ 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;
+static inline move_target_heading __target_heading(move_target_op op, float angle, float distance) {
+ move_target_heading th;
th.operation = op;
th.angle = angle;
th.distance = distance;
#define NO_TURN() __target_heading(TOP_DONT_TURN, 0, 0)
#define ARRIVE_FROM(heading, dist) __target_heading(TOP_ARRIVE_FROM, (heading), (dist))
-static inline struct final_heading target2final_heading(struct move_target_heading th)
-{
- struct final_heading fh;
+static inline final_heading target2final_heading(move_target_heading th) {
+ final_heading fh;
switch (th.operation) {
case TOP_DONT_TURN: fh.turn_type = FH_DONT_TURN; break;
/* Represents the target position of a move. */
struct move_target {
double x, y;
- struct move_target_heading heading;
- struct TrajectoryConstraints tc; /* if (trajectory) taken from it */
+ move_target_heading heading;
+ TrajectoryConstraints tc; /* if (trajectory) taken from it */
bool use_planning; /* FIXME (FJ): this variable is not used anywhere? */
/** 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;
+ struct Trajectory *trajectory;
};
extern struct TrajectoryConstraints trajectoryConstraintsDefault;
-#ifdef __cplusplus
-extern "C" {
-#endif
-
/* extern double target_x, target_y; */
-static inline double __trans_ang(double phi)
-{
- if (robot.team_color == BLUE) {
- return phi;
- } else {
- double a;
- a = M_PI/2.0 - phi + M_PI/2.0;
- a = fmod(a, 2.0*M_PI);
- if (phi >= 0 && a >= 0) a -= 2.0*M_PI;
- if (phi < 0 && a < 0) a += 2.0*M_PI;
- return a;
- }
-}
-
-static inline struct move_target_heading __trans_heading(struct move_target_heading h)
-{
- if (robot.team_color == BLUE) {
- return h;
- } else {
- if (h.operation != TOP_DONT_TURN) {
- h.angle = __trans_ang(h.angle);
- switch (h.operation) {
- case FH_CW:
- h.operation = TOP_TURN_CCW;
- break;
- case FH_CCW:
- h.operation = TOP_TURN_CW;
- break;
- default:
- break;
- }
- }
- return h;
- }
-}
-
-static inline double __trans_x(double x)
-{
- if (robot.team_color == BLUE)
- return x;
- else
- return PLAYGROUND_WIDTH_M - x;
-}
+double trans_angle(double phi);
-static inline double __trans_y(double y)
-{
- return y;
-}
-
-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 trans_x(double x); //BLUE
+double trans_y(double y);
extern bool init_ekf_flag;
-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 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();
-
-/** 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)
-
-/** 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)
+class MoveHelper
{
- if (x) {
- *x = __trans_x(*x);
+ struct Trajectory *t;
+ move_target_heading trans_heading(move_target_heading h);
+ public:
+ /* Low-level trajectory handling */
+ MoveHelper& trajectory_new(TrajectoryConstraints *tc, bool backward = false);
+ MoveHelper& trajectory_new_backward(TrajectoryConstraints *tc) {
+ return trajectory_new(tc, true);
}
- if (phi) {
- *phi = __trans_ang(*phi);
+ MoveHelper& add_point_notrans(double x_m, double y_m);
+ MoveHelper& add_final_point_notrans(double x_m, double y_m, move_target_heading heading);
+ MoveHelper& add_point_trans(double x, double y) {
+ return add_point_notrans(trans_x(x), trans_y(y));
}
-}
-
-
-
-#ifdef __cplusplus
-}
-#endif
+ MoveHelper& add_final_point_trans(double x, double y, move_target_heading heading) {
+ return add_final_point_notrans(trans_x(x), trans_y(y), trans_heading(heading));
+ }
+ void send_speed(double left, double right);
+ void goto_notrans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc, bool planning);
+ void move_by(double distance, move_target_heading heading, TrajectoryConstraints *tc);
+ void stop();
+ void goto_trans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc) {
+ goto_notrans(trans_x(x), trans_y(y), trans_heading(heading), tc, true);
+ }
+ void moveto_trans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc) {
+ goto_notrans(trans_x(x), trans_y(y), trans_heading(heading), tc, false);
+ }
+ void move_by_trans(double distance, move_target_heading heading, TrajectoryConstraints *tc) {
+ move_by(distance, trans_heading(heading), tc);
+ }
+ 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);
+};
#endif /* MOVEHELPER_EB2008_H */
projects / eurobot / public.git / commitdiff