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))
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))