Source code for morse.modifiers.imu_noise
import logging; logger = logging.getLogger("morse." + __name__)
import random
from morse.core.modifier import Modifier
[docs]class MorseIMUNoiseClass(Modifier):
[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 IMU Noise modifier. Check component_config.py file.")
self._gyro_std_dev = 0.5
self._accel_std_dev = 0.5
# Extract the Modifier parameters from the dictionary if it is given
try:
self._gyro_std_dev = mod_data[2].get("gyro_std", self._gyro_std_dev)
self._accel_std_dev = mod_data[2].get("accel_std", self._accel_std_dev)
except:
pass
logger.info("Adding noise to IMU with standard deviations: gyro %.4f, accel %.4f", \
self._gyro_std_dev, self._accel_std_dev)
[docs] def noisify(self, component_instance):
for i in range(0, 3):
component_instance.local_data['angular_velocity'][i] = \
random.gauss(component_instance.local_data['angular_velocity'][i], self._gyro_std_dev)
component_instance.local_data['linear_acceleration'][i] = \
random.gauss(component_instance.local_data['linear_acceleration'][i], self._accel_std_dev)