Source code for morse.modifiers.odometry_noise

import logging; logger = logging.getLogger("morse." + __name__)

from morse.core.modifier import Modifier
from math import cos, sin

[docs]class MorseOdometryNoiseClass(Modifier): def __init__(self): super(self.__class__, self).__init__() self._factor = 1.05 self._gyro_drift = 0.00000005 self._drift_x = 0.0 self._drift_y = 0.0 self._drift_yaw = 0.0
[docs] def register_component(self, component_name, component_instance, mod_data): """ Add the corresponding function to a component. """ # Extract the information for this modifier # This will be tailored for each middleware according to its needs function_name = mod_data[1] try: # Get the reference to the function function = getattr(self, function_name) except AttributeError as detail: logger.error("%s. Check the 'component_config.py' file for typos" % detail) return if function_name == "noisify": component_instance.output_modifiers.append(function) else: logger.warning("Unknown function name for Odometry Noise modifier." "Check component_config.py file.") # Extract the Modifier parameters try: self._factor = mod_data[2].get("factor", 1.05) self._gyro_drift = mod_data[2].get("gyro_drift", 0.0) except: pass
[docs] def noisify(self, component_instance): # 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 data = component_instance.local_data self._drift_yaw += self._gyro_drift dx = self._factor * ( data['dx'] * cos(self._drift_yaw) + data['dy'] * sin(self._drift_yaw)) dy = self._factor * ( data['dy'] * cos(self._drift_yaw) - data['dx'] * sin(self._drift_yaw)) self._drift_x += dx - data['dx'] self._drift_y += dy - data['dy'] data['dS'] *= self._factor data['dx'] = dx data['dy'] = dy data['dyaw'] += self._gyro_drift data['x'] += self._drift_x data['y'] += self._drift_y data['yaw'] += self._drift_yaw freq = component_instance.frequency data['vx'] = data['dx'] / freq data['vy'] = data['dy'] / freq data['wz'] = data['dyaw'] / freq