[modifier] Rework odometry_noise to work at the different odometry level

Fix morse-simulator#553

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

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