Replanning of ARRIVE_FROM needs to be fixed.
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() {
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
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 {
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;
}
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;
}
}
+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 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 {
* @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
* @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;
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);
}
+
+
#include <robot.h>
#include <trgenconstr.h>
#include <math.h>
+#include <stdbool.h>
+
+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;
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);
#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)
projects / eurobot / public.git / commitdiff