def __init__(self, name):
rospy.loginfo('Create ' + name + ' Sensor')
self.dt = 1.0 / rospy.get_param(name + '/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name + '/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz", 1.0 / self.dt)
self.cp = np.array(rospy.get_param(name + '/CP',
[0.0, 0.0, 0.0])).reshape(3, 1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0], self.cp[1],
self.cp[2])
self.precision = rospy.get_param(name + '/precision', 0.0001)
rospy.loginfo("\tc.precision: %.1e g", self.precision)
self.std_coeff = rospy.get_param(name + '/std_coeff', 0.1)
rospy.loginfo("\td.noise coeff: %.3f", self.std_coeff)
#Noise Characteristics
self.bs = np.random.normal(
0,
rospy.get_param(name + '/scaleBias', 0.03) / 3, 3)
self.ks = np.random.normal(
0,
rospy.get_param(name + '/scaleThermal', 0.002) / 3, 3)
self.bias = Vector3()
self.randomwalk = Vector3()
self.scale = Vector3()
self.measurement = SimSensor3()
self.temp = 25.0
self.pub = rospy.Publisher(name, SimSensor3)
self.states_sub = rospy.Subscriber("states", SimStates,
self.StatesCallback)
self.atm_sub = rospy.Subscriber("environment", Environment,
self.EnvironmentCallback)
self.real = SimStates()
self.realnew = False
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\te.Loading Thermal Chracteristics")
self.airspeed = 0.0
def __init__(self, name):
rospy.loginfo('Create ' + name + ' Sensor')
self.dt = 1.0 / rospy.get_param(name + '/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name + '/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz", 1.0 / self.dt)
self.cp = np.array(rospy.get_param(name + '/CP',
[0.0, 0.0, 0.0])).reshape(3, 1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0], self.cp[1],
self.cp[2])
self.resolution = rospy.get_param(name + '/resolution', 0.001)
rospy.loginfo("\tc.resolution: %.1e mbar", self.resolution)
self.maxvalue = rospy.get_param(name + '/max', 1500.0)
rospy.loginfo("\td.maxvalue: %.4f mbar", self.maxvalue)
self.minvalue = rospy.get_param(name + '/min', 0.0)
rospy.loginfo("\te.minvalue: %.4f mbar", self.minvalue)
self.std_coeff = rospy.get_param(name + '/std_coeff', 0.1)
rospy.loginfo("\tf.noise coeff: %.3f", self.std_coeff)
self.bar_true = 1013.25
self.temp = 25.0
self.pub = rospy.Publisher(name, SimBarometer)
self.states_sub = rospy.Subscriber("states", SimStates,
self.StatesCallback)
self.atm_sub = rospy.Subscriber("environment", Environment,
self.EnvironmentCallback)
self.measurement = SimBarometer()
self.realnew = False
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\tg.Loading Thermal Chracteristics")
self.airspeed = 0.0
self.temp = 25.0
def __init__(self,name):
rospy.loginfo('Create ' + name +' Sensor')
self.dt = 1.0/rospy.get_param(name+'/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name+'/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz",1.0/self.dt)
self.cp = np.array(rospy.get_param(name+'/CP', [0.0, 0.0, 0.0])).reshape(3,1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0],self.cp[1],self.cp[2])
self.resolution = rospy.get_param(name+'/resolution', 0.0001)
rospy.loginfo("\tc.resolution: %.1e g",self.resolution)
self.maxvalue = rospy.get_param(name+'/max', 16.0)
rospy.loginfo("\td.maxvalue: %.2f g",self.maxvalue)
self.std_coeff = rospy.get_param(name+'/std_coeff', [0.0,0.0,0.0])
rospy.loginfo("\te.noise coeff:")
rospy.loginfo("\t\t1.main std: %.3f",self.std_coeff[0])
rospy.loginfo("\t\t2.bias coeff: %.3f",self.std_coeff[1])
rospy.loginfo("\t\t3.random walk coeff: %.3f",self.std_coeff[2])
#Noise Characteristics
self.bs = np.random.normal(0,rospy.get_param(name+'/scaleBias', 0.03)/3,3)
self.ks = np.random.normal(0,rospy.get_param(name+'/scaleThermal', 0.002)/3,3)
self.bb = np.random.normal(0,rospy.get_param(name+'/offsetBias', 0.785)/3,3)
self.kb = np.random.normal(0,rospy.get_param(name+'/offsetThermal', 0.0084)/3,3)
self.p0 = rospy.get_param(name+'/RWPSD', 0.00392)
self.bias=Vector3()
self.randomwalk=Vector3()
self.scale = Vector3()
self.temp=25.0
self.grav=9.807
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\tf.Loading Thermal Chracteristics")
self.airspeed = 0.0
self.measurement = SimSensor3()
self.pub = rospy.Publisher(name, SimSensor3)
self.states_sub = rospy.Subscriber("states",SimStates,self.StatesCallback)
self.env_sub = rospy.Subscriber("environment",Environment,self.EnvironmentCallback)
self.real=SimStates()
self.realnew=False
def __init__(self,name):
rospy.loginfo('Create ' + name +' Sensor')
self.dt = 1.0/rospy.get_param(name+'/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name+'/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz",1.0/self.dt)
self.cp = np.array(rospy.get_param(name+'/CP', [0.0, 0.0, 0.0])).reshape(3,1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0],self.cp[1],self.cp[2])
self.precision = rospy.get_param(name+'/precision', 0.0001)
rospy.loginfo("\tc.precision: %.1e g",self.precision)
self.std_coeff = rospy.get_param(name+'/std_coeff', 0.1)
rospy.loginfo("\td.noise coeff: %.3f",self.std_coeff)
#Noise Characteristics
self.bs = np.random.normal(0,rospy.get_param(name+'/scaleBias', 0.03)/3,3)
self.ks = np.random.normal(0,rospy.get_param(name+'/scaleThermal', 0.002)/3,3)
self.bias=Vector3()
self.randomwalk=Vector3()
self.scale = Vector3()
self.measurement = SimSensor3()
self.temp=25.0
self.pub = rospy.Publisher(name, SimSensor3)
self.states_sub = rospy.Subscriber("states",SimStates,self.StatesCallback)
self.atm_sub = rospy.Subscriber("environment",Environment,self.EnvironmentCallback)
self.real=SimStates()
self.realnew=False
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\te.Loading Thermal Chracteristics")
self.airspeed = 0.0
def __init__(self,name):
rospy.loginfo('Create ' + name +' Sensor')
self.dt = 1.0/rospy.get_param(name+'/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name+'/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz",1.0/self.dt)
self.cp = np.array(rospy.get_param(name+'/CP', [0.0, 0.0, 0.0])).reshape(3,1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0],self.cp[1],self.cp[2])
self.resolution = rospy.get_param(name+'/resolution', 0.001)
rospy.loginfo("\tc.resolution: %.1e mbar",self.resolution)
self.maxvalue = rospy.get_param(name+'/max', 1500.0)
rospy.loginfo("\td.maxvalue: %.4f mbar",self.maxvalue)
self.minvalue = rospy.get_param(name+'/min', 0.0)
rospy.loginfo("\te.minvalue: %.4f mbar",self.minvalue)
self.std_coeff = rospy.get_param(name+'/std_coeff', 0.1)
rospy.loginfo("\tf.noise coeff: %.3f",self.std_coeff)
self.bar_true=1013.25
self.temp=25.0
self.pub = rospy.Publisher(name, SimBarometer)
self.states_sub = rospy.Subscriber("states",SimStates,self.StatesCallback)
self.atm_sub = rospy.Subscriber("environment",Environment,self.EnvironmentCallback)
self.measurement = SimBarometer()
self.realnew=False
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\tg.Loading Thermal Chracteristics")
self.airspeed = 0.0
self.temp=25.0
class Barometer():
def __init__(self, name):
rospy.loginfo('Create ' + name + ' Sensor')
self.dt = 1.0 / rospy.get_param(name + '/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name + '/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz", 1.0 / self.dt)
self.cp = np.array(rospy.get_param(name + '/CP',
[0.0, 0.0, 0.0])).reshape(3, 1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0], self.cp[1],
self.cp[2])
self.resolution = rospy.get_param(name + '/resolution', 0.001)
rospy.loginfo("\tc.resolution: %.1e mbar", self.resolution)
self.maxvalue = rospy.get_param(name + '/max', 1500.0)
rospy.loginfo("\td.maxvalue: %.4f mbar", self.maxvalue)
self.minvalue = rospy.get_param(name + '/min', 0.0)
rospy.loginfo("\te.minvalue: %.4f mbar", self.minvalue)
self.std_coeff = rospy.get_param(name + '/std_coeff', 0.1)
rospy.loginfo("\tf.noise coeff: %.3f", self.std_coeff)
self.bar_true = 1013.25
self.temp = 25.0
self.pub = rospy.Publisher(name, SimBarometer)
self.states_sub = rospy.Subscriber("states", SimStates,
self.StatesCallback)
self.atm_sub = rospy.Subscriber("environment", Environment,
self.EnvironmentCallback)
self.measurement = SimBarometer()
self.realnew = False
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\tg.Loading Thermal Chracteristics")
self.airspeed = 0.0
self.temp = 25.0
def iterate(self, states):
Reb = quat2Reb(states.pose.orientation)
bar_noise = np.random.normal(0, self.std_coeff, 1)
barpos = states.pose.position.z + np.dot(Reb[2][0:3], self.cp)
self.measurement.pressure = self.bar_true
self.measurement.pressure = saturation(
floor(self.measurement.pressure / self.resolution) *
self.resolution, self.minvalue, self.maxvalue)
self.tempOffset = self.thermalMass.step(self.airspeed, self.dt)
self.measurement.temperature = self.temp + self.tempOffset
def StatesCallback(self, data):
self.real = data
self.realnew = True
def EnvironmentCallback(self, data):
self.bar_true = data.pressure
self.grav = data.gravity
self.temp = data.temperature
if self.realnew:
self.airspeed = np.sqrt(
pow(self.real.velocity.linear.x - data.wind.x, 2) +
pow(self.real.velocity.linear.y - data.wind.y, 2) +
pow(self.real.velocity.linear.z - data.wind.z, 2))
class Magnetometer():
def __init__(self, name):
rospy.loginfo('Create ' + name + ' Sensor')
self.dt = 1.0 / rospy.get_param(name + '/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name + '/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz", 1.0 / self.dt)
self.cp = np.array(rospy.get_param(name + '/CP',
[0.0, 0.0, 0.0])).reshape(3, 1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0], self.cp[1],
self.cp[2])
self.precision = rospy.get_param(name + '/precision', 0.0001)
rospy.loginfo("\tc.precision: %.1e g", self.precision)
self.std_coeff = rospy.get_param(name + '/std_coeff', 0.1)
rospy.loginfo("\td.noise coeff: %.3f", self.std_coeff)
#Noise Characteristics
self.bs = np.random.normal(
0,
rospy.get_param(name + '/scaleBias', 0.03) / 3, 3)
self.ks = np.random.normal(
0,
rospy.get_param(name + '/scaleThermal', 0.002) / 3, 3)
self.bias = Vector3()
self.randomwalk = Vector3()
self.scale = Vector3()
self.measurement = SimSensor3()
self.temp = 25.0
self.pub = rospy.Publisher(name, SimSensor3)
self.states_sub = rospy.Subscriber("states", SimStates,
self.StatesCallback)
self.atm_sub = rospy.Subscriber("environment", Environment,
self.EnvironmentCallback)
self.real = SimStates()
self.realnew = False
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\te.Loading Thermal Chracteristics")
self.airspeed = 0.0
def iterate(self, states):
Reb = quat2Reb(states.pose.orientation)
Mb, dec = getMag(states.geoid.latitude, states.geoid.longitude,
states.geoid.altitude, Reb)
nm_noise = np.random.normal(0, self.std_coeff, 3)
self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)
self.measurement.axis.x = (1 + self.scale.x) * Mb.x + nm_noise[0]
self.measurement.axis.y = (1 + self.scale.y) * Mb.y + nm_noise[1]
self.measurement.axis.z = (1 + self.scale.z) * Mb.z + nm_noise[2]
self.measurement.axis.x = floor(
self.measurement.axis.x / self.precision) * self.precision
self.measurement.axis.y = floor(
self.measurement.axis.y / self.precision) * self.precision
self.measurement.axis.z = floor(
self.measurement.axis.z / self.precision) * self.precision
self.measurement.temperature = self.temp
self.tempOffset = self.thermalMass.step(self.airspeed, self.dt)
self.measurement.temperature = self.temp + self.tempOffset
def StatesCallback(self, data):
self.real = data
self.realnew = True
def EnvironmentCallback(self, data):
self.temp = data.temperature
if self.realnew:
self.airspeed = np.sqrt(
pow(self.real.velocity.linear.x - data.wind.x, 2) +
pow(self.real.velocity.linear.y - data.wind.y, 2) +
pow(self.real.velocity.linear.z - data.wind.z, 2))
class Magnetometer():
def __init__(self,name):
rospy.loginfo('Create ' + name +' Sensor')
self.dt = 1.0/rospy.get_param(name+'/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name+'/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz",1.0/self.dt)
self.cp = np.array(rospy.get_param(name+'/CP', [0.0, 0.0, 0.0])).reshape(3,1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0],self.cp[1],self.cp[2])
self.precision = rospy.get_param(name+'/precision', 0.0001)
rospy.loginfo("\tc.precision: %.1e g",self.precision)
self.std_coeff = rospy.get_param(name+'/std_coeff', 0.1)
rospy.loginfo("\td.noise coeff: %.3f",self.std_coeff)
#Noise Characteristics
self.bs = np.random.normal(0,rospy.get_param(name+'/scaleBias', 0.03)/3,3)
self.ks = np.random.normal(0,rospy.get_param(name+'/scaleThermal', 0.002)/3,3)
self.bias=Vector3()
self.randomwalk=Vector3()
self.scale = Vector3()
self.measurement = SimSensor3()
self.temp=25.0
self.pub = rospy.Publisher(name, SimSensor3)
self.states_sub = rospy.Subscriber("states",SimStates,self.StatesCallback)
self.atm_sub = rospy.Subscriber("environment",Environment,self.EnvironmentCallback)
self.real=SimStates()
self.realnew=False
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\te.Loading Thermal Chracteristics")
self.airspeed = 0.0
def iterate(self,states):
Reb = quat2Reb(states.pose.orientation)
Mb,dec=getMag(states.geoid.latitude,states.geoid.longitude,states.geoid.altitude,Reb)
nm_noise = np.random.normal(0,self.std_coeff,3)
self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)
self.measurement.axis.x=(1+self.scale.x)*Mb.x+nm_noise[0]
self.measurement.axis.y=(1+self.scale.y)*Mb.y+nm_noise[1]
self.measurement.axis.z=(1+self.scale.z)*Mb.z+nm_noise[2]
self.measurement.axis.x = floor(self.measurement.axis.x/self.precision)*self.precision
self.measurement.axis.y = floor(self.measurement.axis.y/self.precision)*self.precision
self.measurement.axis.z = floor(self.measurement.axis.z/self.precision)*self.precision
self.measurement.temperature = self.temp
self.tempOffset = self.thermalMass.step(self.airspeed,self.dt)
self.measurement.temperature = self.temp + self.tempOffset
def StatesCallback(self,data):
self.real=data
self.realnew=True
def EnvironmentCallback(self,data):
self.temp=data.temperature
if self.realnew:
self.airspeed = np.sqrt(pow(self.real.velocity.linear.x - data.wind.x,2) + pow(self.real.velocity.linear.y - data.wind.y,2) + pow(self.real.velocity.linear.z - data.wind.z,2))
class Altimeter():
def __init__(self,name):
rospy.loginfo('Create ' + name +' Sensor')
self.dt = 1.0/rospy.get_param(name+'/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name+'/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz",1.0/self.dt)
self.cp = np.array(rospy.get_param(name+'/CP', [0.0, 0.0, 0.0])).reshape(3,1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0],self.cp[1],self.cp[2])
self.resolution = rospy.get_param(name+'/resolution', 0.001)
rospy.loginfo("\tc.resolution: %.1e meters",self.resolution)
self.maxvalue = rospy.get_param(name+'/max', 1500.0)
rospy.loginfo("\td.maxvalue: %.4f meters",self.maxvalue)
self.minvalue = rospy.get_param(name+'/min', 0.0)
rospy.loginfo("\te.minvalue: %.4f meters",self.minvalue)
self.std_coeff = rospy.get_param(name+'/std_coeff', 0.1)
rospy.loginfo("\tf.noise coeff: %.3f",self.std_coeff)
self.pub = rospy.Publisher(name, SimSensor)
self.states_sub = rospy.Subscriber('states',SimStates,self.StatesCallback)
self.env_sub = rospy.Subscriber("environment",Environment,self.EnvironmentCallback)
self.measurement = SimSensor()
self.realnew=False
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\tg.Loading Thermal Chracteristics")
self.airspeed = 0.0
self.temp=25.0
def iterate(self,states):
Reb = quat2Reb(states.pose.orientation)
if abs(Reb[2][2])<=1e-4:
self.measurement.value=self.maxvalue
else:
self.measurement.value=saturation((-states.pose.position.z-np.dot(Reb[2][0:3],self.cp)/Reb[2][2]),self.minvalue,self.maxvalue)
rf_noise = np.random.normal(0,self.std_coeff,1)
if self.measurement.value<=self.minvalue:
rf_noise = np.random.normal(0,self.minvalue/3.0,1)
if self.measurement.value>=self.maxvalue-1:
rf_noise = np.random.normal(0,self.maxvalue/3.0,1)
self.measurement.value +=rf_noise[0]
self.measurement.value = max(self.minvalue,floor(self.measurement.value/self.resolution)*self.resolution)
self.tempOffset = self.thermalMass.step(self.airspeed,self.dt)
self.measurement.temperature = self.temp + self.tempOffset
def StatesCallback(self,data):
self.real=data
self.realnew=True
def EnvironmentCallback(self,data):
self.grav=data.gravity
self.temp=data.temperature
if self.realnew:
self.airspeed = np.sqrt(pow(self.real.velocity.linear.x - data.wind.x,2) + pow(self.real.velocity.linear.y - data.wind.y,2) + pow(self.real.velocity.linear.z - data.wind.z,2))
def __init__(self, name):
rospy.loginfo('Create ' + name + ' Sensor')
self.dt = 1.0 / rospy.get_param(name + '/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name + '/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz", 1.0 / self.dt)
self.cp = np.array(rospy.get_param(name + '/CP',
[0.0, 0.0, 0.0])).reshape(3, 1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0], self.cp[1],
self.cp[2])
self.resolution = rospy.get_param(name + '/resolution', 0.0001)
rospy.loginfo("\tc.resolution: %.1e g", self.resolution)
self.maxvalue = rospy.get_param(name + '/max', 16.0)
rospy.loginfo("\td.maxvalue: %.2f g", self.maxvalue)
self.std_coeff = rospy.get_param(name + '/std_coeff', [0.0, 0.0, 0.0])
rospy.loginfo("\te.noise coeff:")
rospy.loginfo("\t\t1.main std: %.3f", self.std_coeff[0])
rospy.loginfo("\t\t2.bias coeff: %.3f", self.std_coeff[1])
rospy.loginfo("\t\t3.random walk coeff: %.3f", self.std_coeff[2])
#Noise Characteristics
self.bs = np.random.normal(
0,
rospy.get_param(name + '/scaleBias', 0.03) / 3, 3)
self.ks = np.random.normal(
0,
rospy.get_param(name + '/scaleThermal', 0.002) / 3, 3)
self.bb = np.random.normal(
0,
rospy.get_param(name + '/offsetBias', 0.785) / 3, 3)
self.kb = np.random.normal(
0,
rospy.get_param(name + '/offsetThermal', 0.0084) / 3, 3)
self.p0 = rospy.get_param(name + '/RWPSD', 0.00392)
self.bias = Vector3()
self.randomwalk = Vector3()
self.scale = Vector3()
self.temp = 25.0
self.grav = 9.807
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\tf.Loading Thermal Chracteristics")
self.airspeed = 0.0
self.measurement = SimSensor3()
self.pub = rospy.Publisher(name, SimSensor3)
self.states_sub = rospy.Subscriber("states", SimStates,
self.StatesCallback)
self.env_sub = rospy.Subscriber("environment", Environment,
self.EnvironmentCallback)
self.real = SimStates()
self.realnew = False
class Accelerometer():
def __init__(self, name):
rospy.loginfo('Create ' + name + ' Sensor')
self.dt = 1.0 / rospy.get_param(name + '/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name + '/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz", 1.0 / self.dt)
self.cp = np.array(rospy.get_param(name + '/CP',
[0.0, 0.0, 0.0])).reshape(3, 1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0], self.cp[1],
self.cp[2])
self.resolution = rospy.get_param(name + '/resolution', 0.0001)
rospy.loginfo("\tc.resolution: %.1e g", self.resolution)
self.maxvalue = rospy.get_param(name + '/max', 16.0)
rospy.loginfo("\td.maxvalue: %.2f g", self.maxvalue)
self.std_coeff = rospy.get_param(name + '/std_coeff', [0.0, 0.0, 0.0])
rospy.loginfo("\te.noise coeff:")
rospy.loginfo("\t\t1.main std: %.3f", self.std_coeff[0])
rospy.loginfo("\t\t2.bias coeff: %.3f", self.std_coeff[1])
rospy.loginfo("\t\t3.random walk coeff: %.3f", self.std_coeff[2])
#Noise Characteristics
self.bs = np.random.normal(
0,
rospy.get_param(name + '/scaleBias', 0.03) / 3, 3)
self.ks = np.random.normal(
0,
rospy.get_param(name + '/scaleThermal', 0.002) / 3, 3)
self.bb = np.random.normal(
0,
rospy.get_param(name + '/offsetBias', 0.785) / 3, 3)
self.kb = np.random.normal(
0,
rospy.get_param(name + '/offsetThermal', 0.0084) / 3, 3)
self.p0 = rospy.get_param(name + '/RWPSD', 0.00392)
self.bias = Vector3()
self.randomwalk = Vector3()
self.scale = Vector3()
self.temp = 25.0
self.grav = 9.807
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\tf.Loading Thermal Chracteristics")
self.airspeed = 0.0
self.measurement = SimSensor3()
self.pub = rospy.Publisher(name, SimSensor3)
self.states_sub = rospy.Subscriber("states", SimStates,
self.StatesCallback)
self.env_sub = rospy.Subscriber("environment", Environment,
self.EnvironmentCallback)
self.real = SimStates()
self.realnew = False
def iterate(self, states):
Reb = quat2Reb(states.pose.orientation)
rw_noise = np.random.normal(0, self.p0, 3)
self.randomwalk.x += self.dt * rw_noise[0]
self.randomwalk.y += self.dt * rw_noise[1]
self.randomwalk.z += self.dt * rw_noise[2]
self.bias.x = self.bb[0] + self.kb[0] * (self.temp - 298.15)
self.bias.y = self.bb[1] + self.kb[1] * (self.temp - 298.15)
self.bias.z = self.bb[1] + self.kb[1] * (self.temp - 298.15)
self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)
avel = Vector2Array(states.velocity.angular)
aveldot = Vector2Array(states.acceleration.angular)
missal = np.cross(avel, np.cross(avel, self.cp.T)) + np.cross(
aveldot, self.cp.T)
self.measurement.axis.x = (1 + self.scale.x) * (
states.acceleration.linear.x / self.grav + missal[0][0] +
self.grav * Reb[2][0]) + self.bias.x + self.randomwalk.x
self.measurement.axis.y = (1 + self.scale.y) * (
states.acceleration.linear.y / self.grav + missal[0][1] +
self.grav * Reb[2][1]) + self.bias.y + self.randomwalk.y
self.measurement.axis.z = (1 + self.scale.z) * (
states.acceleration.linear.z / self.grav + missal[0][2] +
self.grav * Reb[2][2]) + self.bias.z + self.randomwalk.z
self.measurement.axis.x = saturation(
floor(self.measurement.axis.x / self.resolution) * self.resolution,
-self.maxvalue * self.grav, self.maxvalue * self.grav)
self.measurement.axis.y = saturation(
floor(self.measurement.axis.y / self.resolution) * self.resolution,
-self.maxvalue * self.grav, self.maxvalue * self.grav)
self.measurement.axis.z = saturation(
floor(self.measurement.axis.z / self.resolution) * self.resolution,
-self.maxvalue * self.grav, self.maxvalue * self.grav)
self.tempOffset = self.thermalMass.step(self.airspeed, self.dt)
self.measurement.temperature = self.temp + self.tempOffset
def StatesCallback(self, data):
self.real = data
self.realnew = True
def EnvironmentCallback(self, data):
self.grav = data.gravity
self.temp = data.temperature
if self.realnew:
self.airspeed = np.sqrt(
pow(self.real.velocity.linear.x - data.wind.x, 2) +
pow(self.real.velocity.linear.y - data.wind.y, 2) +
pow(self.real.velocity.linear.z - data.wind.z, 2))
class Accelerometer():
def __init__(self,name):
rospy.loginfo('Create ' + name +' Sensor')
self.dt = 1.0/rospy.get_param(name+'/rate', 100.0)
self.rate = rospy.Rate(rospy.get_param(name+'/rate', 100.0))
rospy.loginfo("\ta.rate: %.1f Hz",1.0/self.dt)
self.cp = np.array(rospy.get_param(name+'/CP', [0.0, 0.0, 0.0])).reshape(3,1)
rospy.loginfo("\tb.position: [%.2f,%.2f,%.2f]", self.cp[0],self.cp[1],self.cp[2])
self.resolution = rospy.get_param(name+'/resolution', 0.0001)
rospy.loginfo("\tc.resolution: %.1e g",self.resolution)
self.maxvalue = rospy.get_param(name+'/max', 16.0)
rospy.loginfo("\td.maxvalue: %.2f g",self.maxvalue)
self.std_coeff = rospy.get_param(name+'/std_coeff', [0.0,0.0,0.0])
rospy.loginfo("\te.noise coeff:")
rospy.loginfo("\t\t1.main std: %.3f",self.std_coeff[0])
rospy.loginfo("\t\t2.bias coeff: %.3f",self.std_coeff[1])
rospy.loginfo("\t\t3.random walk coeff: %.3f",self.std_coeff[2])
#Noise Characteristics
self.bs = np.random.normal(0,rospy.get_param(name+'/scaleBias', 0.03)/3,3)
self.ks = np.random.normal(0,rospy.get_param(name+'/scaleThermal', 0.002)/3,3)
self.bb = np.random.normal(0,rospy.get_param(name+'/offsetBias', 0.785)/3,3)
self.kb = np.random.normal(0,rospy.get_param(name+'/offsetThermal', 0.0084)/3,3)
self.p0 = rospy.get_param(name+'/RWPSD', 0.00392)
self.bias=Vector3()
self.randomwalk=Vector3()
self.scale = Vector3()
self.temp=25.0
self.grav=9.807
#Thermal Chracteristics
self.thermalMass = ThermalRise(name)
rospy.loginfo("\tf.Loading Thermal Chracteristics")
self.airspeed = 0.0
self.measurement = SimSensor3()
self.pub = rospy.Publisher(name, SimSensor3)
self.states_sub = rospy.Subscriber("states",SimStates,self.StatesCallback)
self.env_sub = rospy.Subscriber("environment",Environment,self.EnvironmentCallback)
self.real=SimStates()
self.realnew=False
def iterate(self,states):
Reb = quat2Reb(states.pose.orientation)
rw_noise = np.random.normal(0,self.p0,3)
self.randomwalk.x +=self.dt*rw_noise[0]
self.randomwalk.y +=self.dt*rw_noise[1]
self.randomwalk.z +=self.dt*rw_noise[2]
self.bias.x = self.bb[0] + self.kb[0] * (self.temp - 298.15)
self.bias.y = self.bb[1] + self.kb[1] * (self.temp - 298.15)
self.bias.z = self.bb[1] + self.kb[1] * (self.temp - 298.15)
self.scale.x = self.bs[0] + self.ks[0] * (self.temp - 298.15)
self.scale.y = self.bs[1] + self.ks[1] * (self.temp - 298.15)
self.scale.z = self.bs[2] + self.ks[2] * (self.temp - 298.15)
avel = Vector2Array(states.velocity.angular)
aveldot = Vector2Array(states.acceleration.angular)
missal = np.cross(avel,np.cross(avel,self.cp.T)) + np.cross(aveldot,self.cp.T)
self.measurement.axis.x = (1+self.scale.x)*(states.acceleration.linear.x/self.grav + missal[0][0] + self.grav*Reb[2][0]) + self.bias.x + self.randomwalk.x
self.measurement.axis.y = (1+self.scale.y)*(states.acceleration.linear.y/self.grav + missal[0][1] + self.grav*Reb[2][1]) + self.bias.y + self.randomwalk.y
self.measurement.axis.z = (1+self.scale.z)*(states.acceleration.linear.z/self.grav + missal[0][2] + self.grav*Reb[2][2]) + self.bias.z + self.randomwalk.z
self.measurement.axis.x = saturation(floor(self.measurement.axis.x/self.resolution)*self.resolution,-self.maxvalue*self.grav, self.maxvalue*self.grav)
self.measurement.axis.y = saturation(floor(self.measurement.axis.y/self.resolution)*self.resolution,-self.maxvalue*self.grav, self.maxvalue*self.grav)
self.measurement.axis.z = saturation(floor(self.measurement.axis.z/self.resolution)*self.resolution,-self.maxvalue*self.grav, self.maxvalue*self.grav)
self.tempOffset = self.thermalMass.step(self.airspeed,self.dt)
self.measurement.temperature = self.temp + self.tempOffset
def StatesCallback(self,data):
self.real=data
self.realnew=True
def EnvironmentCallback(self,data):
self.grav=data.gravity
self.temp=data.temperature
if self.realnew:
self.airspeed = np.sqrt(pow(self.real.velocity.linear.x - data.wind.x,2) + pow(self.real.velocity.linear.y - data.wind.y,2) + pow(self.real.velocity.linear.z - data.wind.z,2))
