From 4d21082f9f1823f4a5ff09a53a49b35d29607bee Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Thu, 30 Apr 2009 15:31:48 +0200
Subject: [PATCH] Trajectory controller uses odometry and uzv only just before
 movement start

---
 src/robofsm/fsmmove.cc        | 7 ++++++-
 src/robofsm/motion-control.cc | 6 +++---
 src/robofsm/test/rectangle.cc | 3 +++
 3 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index bdf4f1d1..50a11b0a 100644
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -285,6 +285,12 @@ FSM_STATE(wait_for_command)
 FSM_STATE(movement)
 {
 	switch (FSM_EVENT) {
+		case EV_ENTRY:
+			// TODO lock
+			robot_set_est_pos_notrans(robot.est_pos_uzv.x,
+						  robot.est_pos_uzv.y,
+						  robot.est_pos_uzv.phi);
+			break;
 		case EV_TRAJECTORY_DONE:
 			// Skip close to target because sometimes it turn the robot to much
 // 			FSM_TRANSITION(close_to_target);
@@ -309,7 +315,6 @@ FSM_STATE(movement)
 		case EV_MOVE_STOP:
 			FSM_TRANSITION(wait_for_command);
 			break;
-		case EV_ENTRY:
 		case EV_EXIT:
 			break;
 		case EV_NEW_TARGET:
diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 068803a0..236bdade 100644
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -317,9 +317,9 @@ static void do_control()
 		ROBOT_UNLOCK(ref_pos);
 
 		ROBOT_LOCK(est_pos_uzv);
-		est_pos.x = robot.est_pos_uzv.x;
-		est_pos.y = robot.est_pos_uzv.y;
-		est_pos.phi = robot.est_pos_uzv.phi;
+		est_pos.x = robot.est_pos_odo.x;
+		est_pos.y = robot.est_pos_odo.y;
+		est_pos.phi = robot.est_pos_odo.phi;
 		ROBOT_UNLOCK(est_pos_uzv);
 
 		if (measurement_ok < 2) {
diff --git a/src/robofsm/test/rectangle.cc b/src/robofsm/test/rectangle.cc
index 95e9f6c6..14bd0ea0 100644
--- a/src/robofsm/test/rectangle.cc
+++ b/src/robofsm/test/rectangle.cc
@@ -30,6 +30,9 @@ FSM_STATE(init) {
 			FSM_TRANSITION(rectangle);
 			if (!robot.localization_works) {
 				robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
+			} else {
+				// Set odo position
+				robot_set_est_pos_trans(robot.est_pos_uzv.x, robot.est_pos_uzv.y, DEG2RAD(0));
 			}
 			break;
 		default:;
-- 
2.39.2