From 7b2806f247fe1f448260b132e27db82b4cbfd085 Mon Sep 17 00:00:00 2001
From: Arnaud Degroote <arnaud.degroote@isae.fr>
Date: Tue, 29 Jul 2014 13:52:22 +0200
Subject: [PATCH] [modifier] Rework odometry_noise to work at the different
 odometry level

Fix #553
---
 src/morse/modifiers/odometry_noise.py | 52 +++++++++++++--------------
 src/morse/sensors/odometry.py         | 10 ++++--
 2 files changed, 34 insertions(+), 28 deletions(-)

diff --git a/src/morse/modifiers/odometry_noise.py b/src/morse/modifiers/odometry_noise.py
index 8aa387fae..2518ff841 100644
--- a/src/morse/modifiers/odometry_noise.py
+++ b/src/morse/modifiers/odometry_noise.py
@@ -51,31 +51,31 @@ def modify(self):
         #           factor * dS * cos(yaw)  * sin(drift_yaw)
         #         = factor * ( dx * cos(drift_yaw) +  dy * sin(drift_yaw))
         # Same thing to compute dy
-        try:
-            self._drift_yaw += self._gyro_drift
-            dx = self._factor * ( self.data['dx'] * cos(self._drift_yaw) +
-                                  self.data['dy'] * sin(self._drift_yaw))
-            dy = self._factor * ( self.data['dy'] * cos(self._drift_yaw) -
-                                  self.data['dx'] * sin(self._drift_yaw))
-            
-            self._drift_x +=  dx - self.data['dx']
-            self._drift_y +=  dy - self.data['dy']
-            
+        if self.component_instance.level == "raw":
             self.data['dS'] *= self._factor
-            self.data['dx'] = dx
-            self.data['dy'] = dy
-            self.data['dyaw'] += self._gyro_drift
-            
-            self.data['x'] += self._drift_x
-            self.data['y'] += self._drift_y
-            self.data['yaw'] += self._drift_yaw
-            
-            freq = self.component_instance.frequency
-            
-            self.data['vx'] = self.data['dx'] / freq
-            self.data['vy'] = self.data['dy'] / freq
-            self.data['wz'] = self.data['dyaw'] / freq
-        
-        except KeyError as detail:
-            self.key_error(detail)
+        else:
+            self._drift_yaw += self._gyro_drift
+            real_dx = self.component_instance._dx
+            real_dy = self.component_instance._dy
+            dx = self._factor * ( real_dx * cos(self._drift_yaw) +
+                                  real_dy * sin(self._drift_yaw))
+            dy = self._factor * ( real_dy * cos(self._drift_yaw) -
+                                  real_dx  * sin(self._drift_yaw))
+
+            self._drift_x +=  dx - real_dx
+            self._drift_y +=  dy - real_dy
+
+            if self.component_instance.level == "integrated":
+                self.data['x'] += self._drift_x
+                self.data['y'] += self._drift_y
+                self.data['yaw'] += self._drift_yaw
+
+                freq = self.component_instance.frequency
 
+                self.data['vx'] = real_dx / freq
+                self.data['vy'] = real_dy / freq
+                self.data['wz'] = self.component_instance._dyaw / freq
+            else:
+                self.data['dx'] = dx
+                self.data['dy'] = dy
+                self.data['dyaw'] += self._gyro_drift
diff --git a/src/morse/sensors/odometry.py b/src/morse/sensors/odometry.py
index d4ead75d4..616aa8868 100644
--- a/src/morse/sensors/odometry.py
+++ b/src/morse/sensors/odometry.py
@@ -75,8 +75,10 @@ def default_action(self):
         current_pos = self.original_pos.transformation3d_with(self.position_3d)
 
         # Compute the difference in positions with the previous loop
-        self.local_data['dx'] = current_pos.x - self.previous_pos.x
-        self.local_data['dy'] = current_pos.y - self.previous_pos.y
+        self._dx = current_pos.x - self.previous_pos.x
+        self._dy = current_pos.y - self.previous_pos.y
+        self.local_data['dx'] = self._dx
+        self.local_data['dy'] = self._dy
         self.local_data['dz'] = current_pos.z - self.previous_pos.z
 
         # Compute the difference in orientation with the previous loop
@@ -118,6 +120,10 @@ def default_action(self):
         current_pos = self.original_pos.transformation3d_with(self.position_3d)
 
         # Integrated version
+        self._dx = current_pos.x - self.previous_pos.x
+        self._dy = current_pos.y - self.previous_pos.y
+        self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)
+
         self.local_data['x'] = current_pos.x
         self.local_data['y'] = current_pos.y
         self.local_data['z'] = current_pos.z