def talker(self):
# Class Instances
pressure = FluidPressure()
pressure.variance = 0
temp = Temperature()
temp.variance = 0
depth = Float64()
while not rospy.is_shutdown():
if self.sensor.read():
# Pressure reading (Pascal)
pressure.header.stamp = rospy.get_rostime()
pressure.fluid_pressure = self.sensor.pressure(ms5837.UNITS_Pa)
# Temperature reading (Celsius)
temp.header.stamp = rospy.get_rostime()
temp.temperature = self.sensor.temperature(
ms5837.UNITS_Centigrade)
# Depth reading (Meters)
depth.data = self.sensor.depth(ATM_PRESSURE_PA, LOCAL_GRAVITY)
# Publishing
self.pub_pressure.publish(pressure)
self.pub_temp.publish(temp)
self.pub_depth.publish(depth)
else:
rospy.roswarn('Failed to read.')
rospy.Rate(10).sleep() # 10 hz
def deserialize(self, p_content):
""" see Serializer."""
if len(p_content) < 12:
return None
msg1 = Temperature()
msg2 = RelativeHumidity()
msg3 = FluidPressure()
org = struct.unpack('fff', p_content)
msg1.header.stamp = rospy.Time.now()
# round float value to double.
msg1.temperature = Math.roundFloatToDouble(org[0])
msg1.variance = 0
msg2.header.stamp = rospy.Time.now()
msg2.relative_humidity = Math.roundFloatToDouble(org[1])
msg2.variance = 0
msg3.header.stamp = rospy.Time.now()
msg3.fluid_pressure = Math.roundFloatToDouble(org[2])
msg3.variance = 0
msgs = [msg1, msg2, msg3]
if CommonConfig.DEBUG_FLAG is True:
rospy.logdebug('temperature=' + str(msg1.temperature))
rospy.logdebug('humidity=' + str(msg2.relative_humidity))
rospy.logdebug('pressure=' + str(msg3.fluid_pressure))
return msgs
def pubPressure(self):
msg = FluidPressure()
msg.header.stamp = rospy.Time.now()
msg.fluid_pressure = altimeterObj.get_pressure()
msg.variance = SENSOR_STDEV**2
# rospy.loginfo(msg)
self.pressurePub.publish(msg)
return
def start(self):
self.enable = True
self.fluidPressurePub = rospy.Publisher(self.robot_host +
'/meteorological/pressure',
FluidPressure,
queue_size=10)
self.relativeHumidityPub = rospy.Publisher(self.robot_host +
'/meteorological/humidity',
RelativeHumidity,
queue_size=10)
self.temperaturePub = rospy.Publisher(self.robot_host +
'/meteorological/temperature',
Temperature,
queue_size=10)
sense = SenseHat()
while not rospy.is_shutdown():
pressure = sense.get_pressure()
humidity = sense.get_humidity()
temp = sense.get_temperature()
message_str = "Pressure: %s Millibars which are %s Pascal; Humidity: %s %%rH; Temperature: %s °C " % (
pressure, (pressure * 100), humidity, temp)
rospy.loginfo(message_str)
f = FluidPressure()
f.header.stamp = rospy.Time.now()
f.header.frame_id = "/base_link"
f.fluid_pressure = pressure * 100 # Millibar to Pascal conversion
f.variance = 0
h = RelativeHumidity()
h.header.stamp = rospy.Time.now()
h.header.frame_id = "/base_link"
h.relative_humidity = humidity
h.variance = 0
t = Temperature()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "/base_link"
t.temperature = temp
t.variance = 0
self.fluidPressurePub.publish(f)
self.relativeHumidityPub.publish(h)
self.temperaturePub.publish(t)
self.rate.sleep()
def bar30callback(self, data):
# Check if message id is valid (I'm using SCALED_PRESSURE
# and not SCALED_PRESSURE2)
if data.msgid == 137:
# Transform the payload in a python string
p = pack("QQ", *data.payload64)
# Transform the string in valid values
# https://docs.python.org/2/library/struct.html
time_boot_ms, press_abs, press_diff, temperature = unpack(
"Iffhxx", p)
fp = FluidPressure()
fp.header = data.header
fp.fluid_pressure = press_abs
fp.variance = 2.794
self.abs_pressure = press_abs * 100 # convert hPa to Pa
self.prespub.publish(fp)
def talker(self):
pressure_msg = FluidPressure()
pressure_msg.variance = 0
temp_msg = Temperature()
temp_msg.variance = 0
while not rospy.is_shutdown():
if self.ms5837.read():
pressure_msg.header.stamp = rospy.get_rostime()
pressure_msg.fluid_pressure = self.ms5837.pressure(
ms5837.UNITS_Pa)
temp_msg.header.stamp = rospy.get_rostime()
temp_msg.temperature = self.ms5837.temperature()
self.pub_pressure.publish(pressure_msg)
self.pub_temp.publish(temp_msg)
else:
rospy.roswarn('Failed to read pressure sensor!')
rospy.Rate(10).sleep()
def _create_depth_msg(self):
""" Create depth message from ROV information
Raises:
Exception: No data to create the message
"""
# Check if data is available
if 'SCALED_PRESSURE' not in self.get_data():
raise Exception('no SCALED_PRESSURE data')
msg = FluidPressure()
self._create_header(msg)
pressure_data = self.get_data()['SCALED_PRESSURE']
abs_pressure = pressure_data['press_abs'] * 1.0
msg.fluid_pressure = abs_pressure
diff_pressure = pressure_data['press_diff'] * 1.0
msg.variance = diff_pressure
self.pub.set_data('/depth', msg)
def loop(self):
#Topic 1: PoseWitchCovariance
pwc = PoseWithCovarianceStamped()
pwc.header.stamp = rospy.get_rostime()
pwc.header.frame_id = self.world_frame_id
pwc.pose.pose.position = Coordinates()
pwc.pose.pose.orientation = pypozyx.Quaternion()
cov = pypozyx.PositionError()
status = self.pozyx.doPositioning(pwc.pose.pose.position,
self.dimension, self.height,
self.algorithm, self.tag_device_id)
pozyx.getQuaternion(pwc.pose.pose.orientation, self.tag_device_id)
pozyx.getPositionError(cov, self.tag_device_id)
cov_row1 = [cov.x, cov.xy, cov.xz, 0, 0, 0]
cov_row2 = [cov.xy, cov.y, cov.yz, 0, 0, 0]
cov_row3 = [cov.xz, cov.yz, cov.z, 0, 0, 0]
cov_row4 = [0, 0, 0, 0, 0, 0]
cov_row5 = [0, 0, 0, 0, 0, 0]
cov_row6 = [0, 0, 0, 0, 0, 0]
pwc.pose.covariance = cov_row1 + cov_row2 + cov_row3 + cov_row4 + cov_row5 + cov_row6
#Convert from mm to m
pwc.pose.pose.position.x = pwc.pose.pose.position.x * 0.001
pwc.pose.pose.position.y = pwc.pose.pose.position.y * 0.001
pwc.pose.pose.position.z = pwc.pose.pose.position.z * 0.001
if status == POZYX_SUCCESS:
pub_pose_with_cov.publish(pwc)
#Topic 2: IMU
imu = Imu()
imu.header.stamp = rospy.get_rostime()
imu.header.frame_id = self.tag_frame_id
imu.orientation = pypozyx.Quaternion()
imu.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.angular_velocity = pypozyx.AngularVelocity()
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.linear_acceleration = pypozyx.LinearAcceleration()
imu.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
pozyx.getQuaternion(imu.orientation, self.tag_device_id)
pozyx.getAngularVelocity_dps(imu.angular_velocity, self.tag_device_id)
pozyx.getLinearAcceleration_mg(imu.linear_acceleration,
self.tag_device_id)
#Convert from mg to m/s2
imu.linear_acceleration.x = imu.linear_acceleration.x * 0.0098
imu.linear_acceleration.y = imu.linear_acceleration.y * 0.0098
imu.linear_acceleration.z = imu.linear_acceleration.z * 0.0098
#Convert from Degree/second to rad/s
imu.angular_velocity.x = imu.angular_velocity.x * 0.01745
imu.angular_velocity.y = imu.angular_velocity.y * 0.01745
imu.angular_velocity.z = imu.angular_velocity.z * 0.01745
pub_imu.publish(imu)
#Topic 3: Anchors Info
for i in range(len(anchors)):
dr = AnchorInfo()
dr.header.stamp = rospy.get_rostime()
dr.header.frame_id = self.world_frame_id
dr.id = hex(anchors[i].network_id)
dr.position.x = (float)(anchors[i].pos.x) * 0.001
dr.position.y = (float)(anchors[i].pos.y) * 0.001
dr.position.z = (float)(anchors[i].pos.z) * 0.001
iter_ranging = 0
while iter_ranging < self.do_ranging_attempts:
device_range = DeviceRange()
status = self.pozyx.doRanging(self.anchors[i].network_id,
device_range, self.tag_device_id)
dr.distance = (float)(device_range.distance) * 0.001
dr.RSS = device_range.RSS
if status == POZYX_SUCCESS:
dr.status = True
self.range_error_counts[i] = 0
iter_ranging = self.do_ranging_attempts
else:
dr.status = False
self.range_error_counts[i] += 1
if self.range_error_counts[i] > 9:
self.range_error_counts[i] = 0
rospy.logerr("Anchor %d (%s) lost", i, dr.id)
iter_ranging += 1
# device_range = DeviceRange()
# status = self.pozyx.doRanging(self.anchors[i].network_id, device_range)
# dr.distance = (float)(device_range.distance) * 0.001
# dr.RSS = device_range.RSS
# if status == POZYX_SUCCESS:
# dr.status = True
# self.range_error_counts[i] = 0
# else:
# status = self.pozyx.doRanging(self.anchors[i].network_id, device_range)
# dr.distance = (float)(device_range.distance) * 0.001
# dr.RSS = device_range.RSS
# if status == POZYX_SUCCESS:
# dr.status = True
# self.range_error_counts[i] = 0
# else:
# dr.status = False
# self.range_error_counts[i] += 1
# if self.range_error_counts[i] > 9:
# self.range_error_counts[i] = 0
# rospy.logerr("Anchor %d (%s) lost", i, dr.id)
dr.child_frame_id = "anchor_" + str(i)
pub_anchor_info[i].publish(dr)
#Topic 4: PoseStamped
ps = PoseStamped()
ps.header.stamp = rospy.get_rostime()
ps.header.frame_id = self.world_frame_id
ps.pose.position = pwc.pose.pose.position
ps.pose.orientation = pwc.pose.pose.orientation
pub_pose.publish(ps)
#Topic 5: Pressure
pr = FluidPressure()
pr.header.stamp = rospy.get_rostime()
pr.header.frame_id = self.tag_frame_id
pressure = pypozyx.Pressure()
pozyx.getPressure_Pa(pressure, self.tag_device_id)
pr.fluid_pressure = pressure.value
pr.variance = 0
pub_pressure.publish(pr)
def publishDVLdata():
global unknown
global backupjson
#Get JSON Data
getJson = cycleDVL()
try:
theData = json.loads(getJson)
except:
theData = json.loads(backupjson)
#Alternating data each time data is recieved. Each with unique ID(4123 and 8219) and dataset. Using IF to sort this out
#8219 seem to have additional information like Status, Speed of Sound, Temperature
#More data if more modes of tracking is turned on. IDs 4125 and 8221 belongs to Water Tracking mode.
#IDs 4123 and 8219 belongs to Bottom Track mode.
pubBottom = rospy.Publisher('manta/dvl', DVL, queue_size=10)
pubOdo = rospy.Publisher('nav_msgs/Odometry', Odometry, queue_size=10)
pubPressure = rospy.Publisher('manta/Pressure', FluidPressure, queue_size=10)
#pubWater = rospy.Publisher('sensors/dvl/water', DVL, queue_size=10)
rospy.init_node('DVL1000', anonymous=False)
rate = rospy.Rate(8) # 8hz
theDVL = DVL()
theDVLBeam = DVLBeam()
theOdo = Odometry()
thePressure = FluidPressure()
#Bottom-Trackingnumpy square
if theData["id"] == 8219:
#Parsing Variables
BottomID = theData["id"]
BottomMode = theData["name"]
BottomTime = theData["timeStampStr"]
BottomStatus = theData["frames"][1]["inputs"][0]["lines"][0]["data"][0]
#Speed of Sound variables
BottomSpeedOfSoundMin = theData["frames"][2]["inputs"][0]["min"]
BottomSpeedOfSoundMax = theData["frames"][2]["inputs"][0]["max"]
BottomSpeedOfSoundUnit = theData["frames"][2]["inputs"][0]["units"]
BottomSpeedOfSoundData = theData["frames"][2]["inputs"][0]["lines"][0]["data"][0]
#Temperature varliables
BottomTempMin = theData["frames"][3]["inputs"][0]["min"]
BottomTempMax = theData["frames"][3]["inputs"][0]["max"]
BottomTempUnit = theData["frames"][3]["inputs"][0]["units"]
BottomTempData = theData["frames"][3]["inputs"][0]["lines"][0]["data"][0]
#Pressure variables
BottomPressureName = theData["frames"][4]["inputs"][0]["name"]
BottomPressureMin = theData["frames"][4]["inputs"][0]["min"]
BottomPressureMax = theData["frames"][4]["inputs"][0]["max"]
BottomPressureData = theData["frames"][4]["inputs"][0]["lines"][0]["data"][0]
BottomPressureUnit = theData["frames"][4]["inputs"][0]["units"]
#Beam Velocity Variables
BottomBeamVelMin = theData["frames"][5]["inputs"][0]["min"]
BottomBeamVelMax = theData["frames"][5]["inputs"][0]["max"]
BottomBeamVelUnit = theData["frames"][5]["inputs"][0]["units"]
BottomBeamVel1Data = theData["frames"][5]["inputs"][0]["lines"][0]["data"][0]
BottomBeamVel2Data = theData["frames"][5]["inputs"][0]["lines"][1]["data"][0]
BottomBeamVel3Data = theData["frames"][5]["inputs"][0]["lines"][2]["data"][0]
BottomBeamVel4Data = theData["frames"][5]["inputs"][0]["lines"][3]["data"][0]
BottomBeamVel1Valid = theData["frames"][5]["inputs"][0]["lines"][0]["valid"]
BottomBeamVel2Valid = theData["frames"][5]["inputs"][0]["lines"][1]["valid"]
BottomBeamVel3Valid = theData["frames"][5]["inputs"][0]["lines"][2]["valid"]
BottomBeamVel4Valid = theData["frames"][5]["inputs"][0]["lines"][3]["valid"]
#Beam FOM Variables
BottomBeamFomMin = theData["frames"][5]["inputs"][1]["min"]
BottomBeamFomMax = theData["frames"][5]["inputs"][1]["max"]
BottomBeamFomUnit = theData["frames"][5]["inputs"][1]["units"]
BottomBeamFom1Data = theData["frames"][5]["inputs"][1]["lines"][0]["data"][0]
BottomBeamFom2Data = theData["frames"][5]["inputs"][1]["lines"][1]["data"][0]
BottomBeamFom3Data = theData["frames"][5]["inputs"][1]["lines"][2]["data"][0]
BottomBeamFom4Data = theData["frames"][5]["inputs"][1]["lines"][3]["data"][0]
BottomBeamFom1Valid = theData["frames"][5]["inputs"][1]["lines"][0]["valid"]
BottomBeamFom2Valid = theData["frames"][5]["inputs"][1]["lines"][1]["valid"]
BottomBeamFom3Valid = theData["frames"][5]["inputs"][1]["lines"][2]["valid"]
BottomBeamFom4Valid = theData["frames"][5]["inputs"][1]["lines"][3]["valid"]
#Beam Dist Variables
BottomBeamDistMin = theData["frames"][5]["inputs"][2]["min"]
BottomBeamDistMax = theData["frames"][5]["inputs"][2]["max"]
BottomBeamDistUnit = theData["frames"][5]["inputs"][2]["units"]
BottomBeamDist1Data = theData["frames"][5]["inputs"][2]["lines"][0]["data"][0]
BottomBeamDist2Data = theData["frames"][5]["inputs"][2]["lines"][1]["data"][0]
BottomBeamDist3Data = theData["frames"][5]["inputs"][2]["lines"][2]["data"][0]
BottomBeamDist4Data = theData["frames"][5]["inputs"][2]["lines"][3]["data"][0]
BottomBeamDist1Valid = theData["frames"][5]["inputs"][2]["lines"][0]["valid"]
BottomBeamDist2Valid = theData["frames"][5]["inputs"][2]["lines"][1]["valid"]
BottomBeamDist3Valid = theData["frames"][5]["inputs"][2]["lines"][2]["valid"]
BottomBeamDist4Valid = theData["frames"][5]["inputs"][2]["lines"][3]["valid"]
#XYZ Velocity Variables
BottomXyzVelMin = theData["frames"][6]["inputs"][0]["min"]
BottomXyzVelMax = theData["frames"][6]["inputs"][0]["max"]
BottomXyzVelUnit = theData["frames"][6]["inputs"][0]["units"]
BottomXyzVel1Data = theData["frames"][6]["inputs"][0]["lines"][0]["data"][0]
BottomXyzVel2Data = theData["frames"][6]["inputs"][0]["lines"][1]["data"][0]
BottomXyzVel3Data = theData["frames"][6]["inputs"][0]["lines"][2]["data"][0]
BottomXyzVel4Data = theData["frames"][6]["inputs"][0]["lines"][3]["data"][0]
BottomXyzVel1Valid = theData["frames"][6]["inputs"][0]["lines"][0]["valid"]
BottomXyzVel2Valid = theData["frames"][6]["inputs"][0]["lines"][1]["valid"]
BottomXyzVel3Valid = theData["frames"][6]["inputs"][0]["lines"][2]["valid"]
BottomXyzVel4Valid = theData["frames"][6]["inputs"][0]["lines"][3]["valid"]
#XYZ FOM Variables
BottomXyzFomMin = theData["frames"][6]["inputs"][1]["min"]
BottomXyzFomMax = theData["frames"][6]["inputs"][1]["max"]
BottomXyzFomUnit = theData["frames"][6]["inputs"][1]["units"]
BottomXyzFom1Data = theData["frames"][6]["inputs"][1]["lines"][0]["data"][0]
BottomXyzFom2Data = theData["frames"][6]["inputs"][1]["lines"][1]["data"][0]
BottomXyzFom3Data = theData["frames"][6]["inputs"][1]["lines"][2]["data"][0]
BottomXyzFom4Data = theData["frames"][6]["inputs"][1]["lines"][3]["data"][0]
BottomXyzFom1Valid = theData["frames"][6]["inputs"][1]["lines"][0]["valid"]
BottomXyzFom2Valid = theData["frames"][6]["inputs"][1]["lines"][1]["valid"]
BottomXyzFom3Valid = theData["frames"][6]["inputs"][1]["lines"][2]["valid"]
BottomXyzFom4Valid = theData["frames"][6]["inputs"][1]["lines"][3]["valid"]
theDVL.header.stamp = rospy.Time.now()
theDVL.header.frame_id = "dvl_link"
theDVL.velocity.x = BottomXyzVel1Data
theDVL.velocity.y = BottomXyzVel2Data
if BottomXyzFom3Data > BottomXyzFom4Data:
theDVL.velocity.z = BottomXyzVel4Data
BottomXyzFomZbest = BottomXyzFom4Data
else:
theDVL.velocity.z = BottomXyzVel3Data
BottomXyzFomZbest = BottomXyzFom3Data
#Doing covariances
velocity_covariance = [BottomXyzFom1Data * BottomXyzFom1Data, unknown, unknown, unknown, BottomXyzFom2Data * BottomXyzFom2Data, unknown, unknown, unknown, BottomXyzFomZbest * BottomXyzFomZbest]
#Feeding message
theDVL.velocity_covariance = velocity_covariance
theDVL.altitude = ((BottomBeamDist1Data + BottomBeamDist2Data + BottomBeamDist3Data + BottomBeamDist4Data) / 4)
#Individual beam data
beam1 = theDVLBeam
beam1.range = BottomBeamDist1Data
beam1.range_covariance = 0.0001 #TODO: find accurate value
beam1.velocity = BottomBeamVel1Data
beam1.velocity_covariance = BottomBeamFom1Data * BottomBeamFom1Data
beam1.pose.header.stamp = rospy.Time.now()
beam1.pose.pose.position.x = 0.283
beam1.pose.pose.position.y = 0.283
beam1.pose.pose.position.z = 0
beam1.pose.pose.orientation.x = 0.211 #Estimating values here. 25 degree tilt and 4 cm from origo
beam1.pose.pose.orientation.y = 0.211
beam1.pose.pose.orientation.z = -0.047
beam1.pose.pose.orientation.w = 0.953
beam2 = theDVLBeam
beam2.range = BottomBeamDist2Data
beam2.range_covariance = 0.0001 #TODO: find accurate value
beam2.velocity = BottomBeamVel2Data
beam2.velocity_covariance = BottomBeamFom2Data * BottomBeamFom2Data
beam2.pose.header.stamp = rospy.Time.now()
beam2.pose.pose.position.x = 0.283
beam2.pose.pose.position.y = -0.283
beam2.pose.pose.position.z = 0
beam2.pose.pose.orientation.x = -0.211 #Estimating values here. 25 degree tilt and 4 cm from origo
beam2.pose.pose.orientation.y = -0.211
beam2.pose.pose.orientation.z = 0.047
beam2.pose.pose.orientation.w = -0.953
beam3 = theDVLBeam
beam3.range = BottomBeamDist3Data
beam3.range_covariance = 0.0001 #TODO: find accurate value
beam3.velocity = BottomBeamVel3Data
beam3.velocity_covariance = BottomBeamFom3Data * BottomBeamFom3Data
beam3.pose.header.stamp = rospy.Time.now()
beam3.pose.pose.position.x = -0.283
beam3.pose.pose.position.y = -0.283
beam3.pose.pose.position.z = 0
beam3.pose.pose.orientation.x = -0.299 #Estimating values here. 25 degree tilt and 4 cm from origo
beam3.pose.pose.orientation.y = 0
beam3.pose.pose.orientation.z = 0.707
beam3.pose.pose.orientation.w = -0.641
beam4 = theDVLBeam
beam4.range = BottomBeamDist4Data
beam4.range_covariance = 0.0001 #TODO: find accurate value
beam4.velocity = BottomBeamVel4Data
beam4.velocity_covariance = BottomBeamFom4Data * BottomBeamFom4Data
beam4.pose.header.stamp = rospy.Time.now()
beam4.pose.pose.position.x = -0.283
beam4.pose.pose.position.y = 0.283
beam4.pose.pose.position.z = 0
beam4.pose.pose.orientation.x = 0 #Estimating values here. 25 degree tilt and 4 cm from origo
beam4.pose.pose.orientation.y = 0.299
beam4.pose.pose.orientation.z = 0.641
beam4.pose.pose.orientation.w = 0.707
theDVL.beams = [beam1, beam2, beam3, beam4]
#Check page 49 for Z1 and Z2 and use FOM to evaluate each,
#Covariance is a matrix with variance in the diagonal fields
#[var(x), cov(x,y) cov(x,z)|cov(x,y), var(y), cov(y,z)|cov(x,z), cov(y,z), var(z)]
#Concluded with using FOB*FOB as an estimate for variance
#Try to get Variance out of FOM
#pub = rospy.Publisher('sensors/dvl/bottom', NORTEK, queue_size=10)
rospy.loginfo("Publishing sensor data from DVL Bottom-Track %s" % rospy.get_time())
pubBottom.publish(theDVL)
backupjson = getJson
#Odometry topic
theOdo.header.stamp = rospy.Time.now()
theOdo.header.frame_id = "dvl_link"
theOdo.child_frame_id = "dvl_link"
theOdo.twist.twist.linear.x = theDVL.velocity.x
theOdo.twist.twist.linear.y = theDVL.velocity.y
theOdo.twist.twist.linear.z = theDVL.velocity.z
theOdo.twist.twist.angular.x = unknown
theOdo.twist.twist.angular.y = unknown
theOdo.twist.twist.angular.z = unknown
theOdo.twist.covariance = [BottomXyzFom1Data * BottomXyzFom1Data, unknown, unknown, unknown, unknown, unknown, unknown, BottomXyzFom2Data * BottomXyzFom2Data, unknown, unknown, unknown, unknown, unknown, unknown, BottomXyzFomZbest * BottomXyzFomZbest, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown, unknown]
pubOdo.publish(theOdo)
#Pressure topic
thePressure.header.stamp = rospy.Time.now()
thePressure.header.frame_id = "dvl_link"
thePressure.fluid_pressure = BottomPressureData * 10000 #Convert dbar to Pascal
thePressure.variance = 30*30 #Should do a more accurate meassurement of the variance
pubPressure.publish(thePressure)
#while not rospy.is_shutdown():
#rospy.loginfo("Publishing sensor data from DVL %s" % rospy.get_time())
#pub.publish(theDVL)
#rate.sleep()
rate.sleep()
imu_pub.publish(imuMsg)
imu_raw_pub.publish(imuRawMsg)
if em7180.gotBarometer():
pressure, temperature = em7180.readBarometer()
altitude = (1.0 - math.pow(pressure / 1013.25, 0.190295)) * 44330
#print(' Altitude = %2.2f m\n' % altitude)
# Set Pressure variables
pressMsg = FluidPressure()
pressMsg.header.stamp = rospy.Time.now()
pressMsg.header.frame_id = "imu_link"
pressMsg.fluid_pressure = pressure
pressMsg.variance = 0
# Set Temperature variables
tempMsg = Temperature()
tempMsg.header.stamp = rospy.Time.now()
tempMsg.header.frame_id = "imu_link"
tempMsg.temperature = temperature
tempMsg.variance = 0
# Set Altitude variables
altMsg = altitude
# publish message
temp_pub.publish(tempMsg)
pressure_pub.publish(pressMsg)
alt_pub.publish(altMsg)
if not pressure_data.init():
print("Sensor could not be initialized")
exit(1)
instant_temperature = rospy.Publisher('Instant/Temperature',
Temperature,
queue_size=1)
pub1 = rospy.Publisher('Altimeter', Float32, queue_size=1)
pub_pressure = rospy.Publisher('sensors/Pressure',
FluidPressure,
queue_size=20)
pub_temp = rospy.Publisher('sensors/Temperature', Temperature, queue_size=20)
rospy.init_node('Pressure_node', anonymous=True)
pressure_message = FluidPressure()
pressure_message.variance = 0
temp_message = Temperature()
temp_message.variance = 0
instant_temp = Temperature()
instant_temp.variance = 0
rate = rospy.Rate(1)
while True:
if not sensor_data.read():
print("Error reading sensor")
exit(1)
instant_temp.header.stamp = rospy.get_rostime()
instant_temp.temperature = sensor_data.temperature()
instant_temperature.publish(instant_temp)