fsmmove: Added ARRIVE_FROM target angle operation and several move helper functions

[eurobot/public.git] / src / robofsm / fsmmove.cc

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index 40da3afe055dcdd8948c595d4aeb0c348b0e1bd1..b6d3972608a3e11a0303a2e38518c658109f1ef9 100644 (file)
--- 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;
 }