From d0f2eeaf80bfdaeb24edd33e924b57caf2df4d95 Mon Sep 17 00:00:00 2001 From: Petr Silhavik Date: Sun, 17 Feb 2013 17:56:09 +0100 Subject: [PATCH] robofsm: MoveHelper - new interface New interface of MoveHelper to simplify and shorten code --- src/robofsm/movehelper.cc | 226 ++++++++++++++++---------------------- src/robofsm/movehelper.h | 150 +++++++------------------ 2 files changed, 137 insertions(+), 239 deletions(-) diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc index e89593eb..b39e79b6 100644 --- a/src/robofsm/movehelper.cc +++ b/src/robofsm/movehelper.cc @@ -22,11 +22,13 @@ #include #include #include +#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 @@ -35,7 +37,7 @@ struct TrajectoryConstraints trajectoryConstraintsDefault = { 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 @@ -45,67 +47,21 @@ struct TrajectoryConstraints trajectoryConstraintsDefault = { }; #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. @@ -113,32 +69,13 @@ void robot_set_est_pos_notrans(double x, double y, double phi) * @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. @@ -152,50 +89,60 @@ bool get_arrive_from_point(double target_x_m, double target_y_m, struct move_tar * 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; @@ -223,40 +170,19 @@ void robot_send_speed(double left, double right) { * * @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)); } /** @@ -266,30 +192,70 @@ void robot_moveto_notrans(double x, double y, * @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 diff --git a/src/robofsm/movehelper.h b/src/robofsm/movehelper.h index d7521d25..e196dae1 100644 --- a/src/robofsm/movehelper.h +++ b/src/robofsm/movehelper.h @@ -15,14 +15,13 @@ enum move_target_op { }; 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; @@ -35,9 +34,8 @@ static inline struct move_target_heading __target_heading(enum move_target_op op #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; @@ -53,128 +51,62 @@ static inline struct final_heading target2final_heading(struct move_target_headi /* 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 */ -- 2.39.2