Use correct start position for trajectory planning without uzv

[eurobot/public.git] / src / robofsm / motion-control.cc

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 081abfca722d0661e510e297601bc8b340eec6a5..815d51b7bde5cd7c4fef644523b5e10c1ce4ec1d 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -539,12 +539,22 @@ void get_future_pos(double rel_time_sec, Pos &pos, double &switch_time)
                // Robot doesn't move, so return current position
                pthread_mutex_unlock(&actual_trajectory_lock);
 
-               ROBOT_LOCK(est_pos_uzv);
-               pos.x = robot.est_pos_uzv.x;
-               pos.y = robot.est_pos_uzv.y;
-               pos.phi = robot.est_pos_uzv.phi;
-               pos.v = 0;
-               pos.omega = 0;
-               ROBOT_UNLOCK(est_pos_uzv);
+               if (robot.localization_works) {
+                       ROBOT_LOCK(est_pos_uzv);
+                       pos.x = robot.est_pos_uzv.x;
+                       pos.y = robot.est_pos_uzv.y;
+                       pos.phi = robot.est_pos_uzv.phi;
+                       pos.v = 0;
+                       pos.omega = 0;
+                       ROBOT_UNLOCK(est_pos_uzv);
+               } else {
+                       ROBOT_LOCK(est_pos_odo);
+                       pos.x = robot.est_pos_odo.x;
+                       pos.y = robot.est_pos_odo.y;
+                       pos.phi = robot.est_pos_odo.phi;
+                       pos.v = 0;
+                       pos.omega = 0;
+                       ROBOT_UNLOCK(est_pos_odo);
+               }
        }
 }