Source code for morse.modifiers.odometry_noise
import logging; logger = logging.getLogger("morse." + __name__)
from morse.core.services import do_service_registration
from morse.modifiers.abstract_modifier import AbstractModifier
from math import cos, sin
from random import gauss
[docs]class OdometryNoiseModifier(AbstractModifier):
"""
This modifier allows to simulate two common issues when calculating odometry :
- an error in the scale factor used to compute the distance from the value
returned by the odometer (parameter **factor**)
- the gyroscope natural drift (parameter **gyro_drift** (rad by tick))
Modified data
-------------
The modifier only accumulate errors for a 2D odometry sensor. It modifies so
the following variables :
- **dS** by the scale factor
- **dx** considering the scale factor and gyroscope drift
- **dy** considering the scale factor and gyroscope drift
- **dyaw** considering the gyroscope drift
- **x** considering the scale factor and gyroscope drift
- **y** considering the scale factor and gyroscope drift
- **yaw** considering the gyroscope drift
- **vx** considering the new **dx**
- **vy** considering the new **dy**
- **wz** considering the new **dyaw**
Available methods
-----------------
- ``noisify``: Simulate drift of gyroscope and possible error in the scale
factor
"""
[docs] def initialize(self):
self._factor = float(self.parameter("factor", default=1.05))
self._factor_sigma = float(self.parameter("factor_sigma", default=0))
self._gyro_drift = float(self.parameter("gyro_drift", default=0))
self._gyro_drift_sigma = float(self.parameter("gyro_drift_sigma", default=0))
self._drift_x = 0.0
self._drift_y = 0.0
self._drift_yaw = 0.0
do_service_registration(self.reset_noise, self.component_instance.name())
[docs] def modify(self):
# Basic 2D odometry implementation dx = dS * sin(yaw) and
# dy = dS * cos(yaw)
# If we have some error on dS and yaw, we have
# dx = factor * dS * sin(yaw + drift_yaw)
# = factor * dS * sin(yaw) * cos(drift_yaw) +
# factor * dS * cos(yaw) * sin(drift_yaw)
# = factor * ( dx * cos(drift_yaw) + dy * sin(drift_yaw))
# Same thing to compute dy
if self.component_instance.level == "raw":
self.data['dS'] *= gauss(self._factor, self._factor_sigma)
else:
factor = gauss(self._factor, self._factor_sigma)
drift = gauss(self._gyro_drift, self._gyro_drift_sigma)
self._drift_yaw += drift
real_dx = self.component_instance._dx
real_dy = self.component_instance._dy
dx = factor * ( real_dx * cos(self._drift_yaw) +
real_dy * sin(self._drift_yaw))
dy = 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'] += drift
[docs] def reset_noise(self):
""" Service allowing to simulate loop-closure """
self._drift_x = 0.0
self._drift_y = 0.0
self._drift_yaw = 0.0