def timerCallback(self, event):
if self.sensor.read():
pressure = self.sensor.pressure()
temp = self.sensor.temperature()
self.depth_raw = self.sensor.depth()
depth = self.depth_raw - self.depth_offset
self.depth = depth
pressure_msg = FluidPressure()
pressure_msg.header.frame_id = 'pressure_sensor'
pressure_msg.header.seq = self.count
pressure_msg.header.stamp = rospy.Time.now()
pressure_msg.fluid_pressure = pressure
self.fluid_pub.publish(pressure_msg)
depth_stamped_msg = Vector3Stamped()
depth_stamped_msg.header.frame_id = 'pressure_sensor'
depth_stamped_msg.header.seq = self.count
depth_stamped_msg.header.stamp = rospy.Time.now() #TODO fix this
depth_stamped_msg.vector.z = depth # TODO this
self.depth_stamped_pub.publish(depth_stamped_msg)
depth_msg = Float64()
depth_msg.data = depth
self.depth_pub.publish(depth_msg)
temp_msg = Temperature()
temp_msg.header.frame_id = 'pressure_sensor'
temp_msg.header.seq = self.count
temp_msg.header.stamp = rospy.Time.now()
temp_msg.temperature = temp
self.temp_pub.publish(temp_msg)
self.count += 1
def cmd_sync_callback(self, msg_pres_int, msg_imu_int):
msg_pres = FluidPressure();
msg_pres.header.stamp = rospy.Time.now()
msg_pres.header.frame_id = msg_pres_int.header.frame_id
msg_pres.fluid_pressure = (msg_pres_int.fluid_pressure * 1000) - self.atm_press
self.pub_pres.publish(msg_pres)
msg_imu = Imu();
msg_imu.header.stamp = rospy.Time.now()
msg_imu.header.frame_id = "/mur/imu_link"
qx = msg_imu_int.orientation.x
qy = msg_imu_int.orientation.y
qz = msg_imu_int.orientation.z
qw = msg_imu_int.orientation.w
euler_angles = euler_from_quaternion(np.array([qx,qy,qz,qw]))
r = euler_angles[1]
p = -euler_angles[0]
y = euler_angles[2]
q = quaternion_from_euler(r,p,y)
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
msg_imu.orientation.w = q[3]
msg_imu.orientation_covariance = np.array([0,0,0,0,0,0,0,0,0])
msg_imu.angular_velocity.x = -msg_imu_int.angular_velocity.x
msg_imu.angular_velocity.y = msg_imu_int.angular_velocity.y
msg_imu.angular_velocity.z = msg_imu_int.angular_velocity.z
msg_imu.angular_velocity_covariance = np.array([1.2184E-7,0.0,0.0,0.0,1.2184E-7,0.0,0.0,0.0,1.2184E-7])
msg_imu.linear_acceleration.x = msg_imu_int.linear_acceleration.x # The same configuration in the PIXHAWK
msg_imu.linear_acceleration.y = msg_imu_int.linear_acceleration.y
msg_imu.linear_acceleration.z = msg_imu_int.linear_acceleration.z
msg_imu.linear_acceleration_covariance = np.array([9.0E-8,0.0,0.0,0.0,9.0E-8,0.0,0.0,0.0,9.0E-8])
self.pub_imu.publish(msg_imu)
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 __init__(self):
self.node_name = 'thruster_manager'
self.tranform_matrix = np.array([
# x y z roll pitch yaw
[0, 0, 1, 1, -1, 0], # left front motor
[0, 0, -1, 1, 1, 0], # right front motor
[-1, 0, 0, 0, 0, 1], # left rear motor
[1, 0, 0, 0, 0, 1], # right rear motor
[0, 0, -1, 0, -1, 0] # rear motor
])
self.output = np.array([1488.0] * len(self.tranform_matrix))
rospy.init_node(self.node_name)
rospy.Subscriber(self.node_name + '/input', Twist, self.input_callback)
pressure_pub = rospy.Publisher('pressure',
FluidPressure,
queue_size=10)
serial_port = rospy.get_param('~port')
while True:
try:
ser = serial.Serial(serial_port, 19200, timeout=1)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
ser.write(
(','.join(str(int(n)) for n in self.output.ravel()) +
'\n').encode('utf-8'))
message = ser.readline().decode('utf-8').strip()
if message.startswith('message'):
print(message)
elif message.startswith('pressure'):
try:
data = float(message.split(':')[1]) # in mBar
message = FluidPressure()
message.fluid_pressure = data / 10 # in kPa
pressure_pub.publish(message)
except Exception as e:
print(e)
rate.sleep()
except serial.serialutil.SerialException as e:
print(e)
try:
ser.close()
except Exception:
pass
time.sleep(0.3)
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 publishBarometerData():
airPressurePub = rospy.Publisher('air_pressure',
FluidPressure,
queue_size=10)
temperaturePub = rospy.Publisher('barometer_temperature',
Temperature,
queue_size=10)
rospy.init_node('barometer_publisher', anonymous=True)
# Set update rate, in Hz
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# Pressure
baro.refreshPressure()
time.sleep(0.1)
baro.readPressure()
# Temperature
baro.refreshTemperature()
time.sleep(0.1)
baro.readTemperature()
# Process the data
baro.calculatePressureAndTemperature()
# Generate Header
header = Header()
header.stamp = rospy.Time.now()
# Generate Message (Temperature)
temperatureMsg = Temperature()
temperatureMsg.header = header
temperatureMsg.temperature = baro.TEMP
temperaturePub.publish(temperatureMsg)
# Generate message and publish (Pressure)
airPressureMsg = FluidPressure()
airPressureMsg.header = header
airPressureMsg.fluid_pressure = baro.PRES * 100
airPressurePub.publish(airPressureMsg)
# Sleep
rate.sleep()
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 sub_imuCB(msg_in): global pub_imu global pub_mag global pub_temp global pub_baro global msg_imu msg_imu.header.stamp = msg_in.header.stamp msg_imu.header.frame_id = msg_in.header.frame_id msg_imu.angular_velocity.x = msg_in.Accel.x msg_imu.angular_velocity.y = msg_in.Accel.y msg_imu.angular_velocity.z = msg_in.Accel.z msg_imu.linear_acceleration.x = msg_in.Gyro.x msg_imu.linear_acceleration.y = msg_in.Gyro.y msg_imu.linear_acceleration.z = msg_in.Gyro.z pub_imu.publish(msg_imu) msg_mag = MagneticField() msg_mag.header.stamp = msg_in.header.stamp msg_mag.header.frame_id = msg_in.header.frame_id msg_mag.magnetic_field.x = msg_in.Mag.x msg_mag.magnetic_field.y = msg_in.Mag.y msg_mag.magnetic_field.z = msg_in.Mag.z pub_mag.publish(msg_mag) msg_temp = Temperature() msg_temp.header.stamp = msg_in.header.stamp msg_temp.header.frame_id = msg_in.header.frame_id msg_temp.temperature = msg_in.Temp pub_temp.publish(msg_temp) msg_baro = FluidPressure() msg_baro.header.stamp = msg_in.header.stamp msg_baro.header.frame_id = msg_in.header.frame_id msg_baro.fluid_pressure = msg_in.Pressure / 1000.0 pub_baro.publish(msg_baro)
def sub_imuCB(msg_in):
global pub_imu
global pub_mag
global pub_temp
global pub_baro
global msg_imu
msg_imu.header.stamp = msg_in.header.stamp
msg_imu.header.frame_id = msg_in.header.frame_id
msg_imu.angular_velocity.x = msg_in.Accel.x
msg_imu.angular_velocity.y = msg_in.Accel.y
msg_imu.angular_velocity.z = msg_in.Accel.z
msg_imu.linear_acceleration.x = msg_in.Gyro.x
msg_imu.linear_acceleration.y = msg_in.Gyro.y
msg_imu.linear_acceleration.z = msg_in.Gyro.z
pub_imu.publish(msg_imu)
msg_mag = MagneticField()
msg_mag.header.stamp = msg_in.header.stamp
msg_mag.header.frame_id = msg_in.header.frame_id
msg_mag.magnetic_field.x = msg_in.Mag.x
msg_mag.magnetic_field.y = msg_in.Mag.y
msg_mag.magnetic_field.z = msg_in.Mag.z
pub_mag.publish(msg_mag)
msg_temp = Temperature()
msg_temp.header.stamp = msg_in.header.stamp
msg_temp.header.frame_id = msg_in.header.frame_id
msg_temp.temperature = msg_in.Temp
pub_temp.publish(msg_temp)
msg_baro = FluidPressure()
msg_baro.header.stamp = msg_in.header.stamp
msg_baro.header.frame_id = msg_in.header.frame_id
msg_baro.fluid_pressure = msg_in.Pressure / 1000.0
pub_baro.publish(msg_baro)
def spin_once(self):
def baroPressureToHeight(value):
c1 = 44330.0
c2 = 9.869232667160128300024673081668e-6
c3 = 0.1901975534856
intermediate = 1 - math.pow(c2 * value, c3)
height = c1 * intermediate
return height
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp_data = data.get('Temperature') # float
orient_data = data.get('Orientation Data')
velocity_data = data.get('Velocity')
position_data = data.get('Latlon')
altitude_data = data.get('Altitude')
acc_data = data.get('Acceleration')
gyr_data = data.get('Angular Velocity')
mag_data = data.get('Magnetic')
pressure_data = data.get('Pressure')
time_data = data.get('Timestamp')
gnss_data = data.get('GNSS')
# debug the data from the sensor
# print(data)
# print("\n")
# create messages and default values
"Temp message"
temp_msg = Temperature()
pub_temp = False
"Imu message supported with Modes 1 & 2"
imuraw_msg = Imu()
pub_imuraw = False
imuins_msg = Imu()
pub_imuins = False
"SensorSample message supported with Mode 2"
ss_msg = sensorSample()
pub_ss = False
"Mag message supported with Modes 1 & 2"
mag_msg = Vector3Stamped()
pub_mag = False
"Baro in meters"
baro_msg = FluidPressure()
height_msg = baroSample()
pub_baro = False
"GNSS message supported only with MTi-G-7xx devices"
"Valid only for modes 1 and 2"
gnssPvt_msg = gnssSample()
gps1_msg = NavSatFix()
gps2_msg = GPSFix()
pub_gnssPvt = False
gnssSatinfo_msg = GPSStatus()
pub_gnssSatinfo = False
# All filter related outputs
"Supported in mode 3"
ori_msg = orientationEstimate()
pub_ori = False
"Supported in mode 3 for MTi-G-7xx devices"
vel_msg = velocityEstimate()
pub_vel = False
"Supported in mode 3 for MTi-G-7xx devices"
pos_msg = positionEstimate()
pub_pos = False
# first getting the sampleTimeFine
# note if we are not using ros time, the we should replace the header
# with the time supplied by the GNSS unit
if time_data and not self.use_rostime:
time = time_data['SampleTimeFine']
h.stamp.secs = 100e-6 * time
h.stamp.nsecs = 1e5 * time - 1e9 * math.floor(h.stamp.secs)
# temp data
if temp_data:
temp_msg.temperature = temp_data['Temp']
pub_temp = True
# acceleration data
if acc_data:
if 'accX' in acc_data: # found acceleration
pub_imuraw = True
imuraw_msg.linear_acceleration.x = acc_data['accX']
imuraw_msg.linear_acceleration.y = acc_data['accY']
imuraw_msg.linear_acceleration.z = acc_data['accZ']
if 'freeAccX' in acc_data: # found free acceleration
pub_imuins = True
imuins_msg.linear_acceleration.x = acc_data['freeAccX']
imuins_msg.linear_acceleration.y = acc_data['freeAccY']
imuins_msg.linear_acceleration.z = acc_data['freeAccZ']
if 'Delta v.x' in acc_data: # found delta-v's
pub_ss = True
ss_msg.internal.imu.dv.x = acc_data['Delta v.x']
ss_msg.internal.imu.dv.y = acc_data['Delta v.y']
ss_msg.internal.imu.dv.z = acc_data['Delta v.z']
#else:
# raise MTException("Unsupported message in XDI_AccelerationGroup.")
# gyro data
if gyr_data:
if 'gyrX' in gyr_data: # found rate of turn
pub_imuraw = True
imuraw_msg.angular_velocity.x = gyr_data['gyrX']
imuraw_msg.angular_velocity.y = gyr_data['gyrY']
imuraw_msg.angular_velocity.z = gyr_data['gyrZ']
# note we do not force publishing the INS if we do not use the free acceleration
imuins_msg.angular_velocity.x = gyr_data['gyrX']
imuins_msg.angular_velocity.y = gyr_data['gyrY']
imuins_msg.angular_velocity.z = gyr_data['gyrZ']
if 'Delta q0' in gyr_data: # found delta-q's
pub_ss = True
ss_msg.internal.imu.dq.w = gyr_data['Delta q0']
ss_msg.internal.imu.dq.x = gyr_data['Delta q1']
ss_msg.internal.imu.dq.y = gyr_data['Delta q2']
ss_msg.internal.imu.dq.z = gyr_data['Delta q3']
#else:
# raise MTException("Unsupported message in XDI_AngularVelocityGroup.")
# magfield
if mag_data:
ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX']
ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY']
ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ']
pub_mag = True
if pressure_data:
pub_baro = True
baro_msg.fluid_pressure = pressure_data['Pressure']
height = baroPressureToHeight(pressure_data['Pressure'])
height_msg.height = ss_msg.internal.baro.height = height
# gps fix message
if gnss_data and 'lat' in gnss_data:
pub_gnssPvt = True
# A "3" means that the MTi-G is using the GPS data.
# A "1" means that the MTi-G was using GPS data and is now coasting/dead-reckoning the
# position based on the inertial sensors (the MTi-G is not using GPS data in this mode).
# This is done for 45 seconds, before the MTi-G Mode drops to "0".
# A "0" means that the MTi-G doesn't use GPS data and also that it
# doesn't output position based on the inertial sensors.
if gnss_data['fix'] < 2:
gnssSatinfo_msg.status = NavSatStatus.STATUS_NO_FIX # no fix
gps1_msg.status.status = NavSatStatus.STATUS_NO_FIX # no fix
gps1_msg.status.service = 0
else:
gnssSatinfo_msg.status = NavSatStatus.STATUS_FIX # unaugmented
gps1_msg.status.status = NavSatStatus.STATUS_FIX # unaugmented
gps1_msg.status.service = NavSatStatus.SERVICE_GPS
# lat lon alt
gps1_msg.latitude = gnss_data['lat']
gps1_msg.longitude = gnss_data['lon']
gps1_msg.altitude = gnss_data['hEll']
# covariances
gps1_msg.position_covariance[0] = math.pow(gnss_data['horzAcc'], 2)
gps1_msg.position_covariance[4] = math.pow(gnss_data['horzAcc'], 2)
gps1_msg.position_covariance[8] = math.pow(gnss_data['vertAcc'], 2)
gps1_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
# custom message
gnssPvt_msg.itow = gnss_data['iTOW']
gnssPvt_msg.fix = gnss_data['fix']
gnssPvt_msg.latitude = gnss_data['lat']
gnssPvt_msg.longitude = gnss_data['lon']
gnssPvt_msg.hEll = gnss_data['hEll']
gnssPvt_msg.hMsl = gnss_data['hMsl']
gnssPvt_msg.vel.x = gnss_data['velE']
gnssPvt_msg.vel.y = gnss_data['velN']
gnssPvt_msg.vel.z = gnss_data['velD']
gnssPvt_msg.hAcc = gnss_data['horzAcc']
gnssPvt_msg.vAcc = gnss_data['vertAcc']
gnssPvt_msg.sAcc = gnss_data['speedAcc']
gnssPvt_msg.pDop = gnss_data['PDOP']
gnssPvt_msg.hDop = gnss_data['HDOP']
gnssPvt_msg.vDop = gnss_data['VDOP']
gnssPvt_msg.numSat = gnss_data['nSat']
gnssPvt_msg.heading = gnss_data['heading']
gnssPvt_msg.headingAcc = gnss_data['headingAcc']
if orient_data:
if 'Q0' in orient_data:
pub_imuraw = True
imuraw_msg.orientation.w = orient_data['Q0']
imuraw_msg.orientation.x = orient_data['Q1']
imuraw_msg.orientation.y = orient_data['Q2']
imuraw_msg.orientation.z = orient_data['Q3']
pub_imuins = True
imuins_msg.orientation.w = orient_data['Q0']
imuins_msg.orientation.x = orient_data['Q1']
imuins_msg.orientation.y = orient_data['Q2']
imuins_msg.orientation.z = orient_data['Q3']
elif 'Roll' in orient_data:
pub_ori = True
ori_msg.roll = orient_data['Roll']
ori_msg.pitch = orient_data['Pitch']
ori_msg.yaw = orient_data['Yaw']
else:
raise MTException(
'Unsupported message in XDI_OrientationGroup')
if velocity_data:
pub_vel = True
vel_msg.velE = velocity_data['velX']
vel_msg.velN = velocity_data['velY']
vel_msg.velU = velocity_data['velZ']
if position_data:
pub_pos = True
pos_msg.latitude = position_data['lat']
pos_msg.longitude = position_data['lon']
if altitude_data:
pub_pos = True
tempData = altitude_data['ellipsoid']
pos_msg.hEll = tempData[0]
# publish available information
if pub_imuraw:
imuraw_msg.header = h
self.imuraw_pub.publish(imuraw_msg)
if pub_imuins:
imuins_msg.header = h
self.imuins_pub.publish(imuins_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
temp_msg.header = h
self.temp_pub.publish(temp_msg)
if pub_ss:
ss_msg.header = h
self.ss_pub.publish(ss_msg)
if pub_baro:
baro_msg.header = h
height_msg.header = h
self.baro_pub.publish(baro_msg)
self.height_pub.publish(height_msg)
if pub_gnssPvt:
gnssPvt_msg.header = h
gps1_msg.header = h
self.gnssPvt_pub.publish(gnssPvt_msg)
self.gps1_pub.publish(gps1_msg)
#if pub_gnssSatinfo:
# gnssSatinfo_msg.header = h
# self.gnssSatinfo_pub.publish(gnssSatinfo_msg)
if pub_ori:
ori_msg.header = h
self.ori_pub.publish(ori_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_pos:
pos_msg.header = h
self.pos_pub.publish(pos_msg)
#Add noise if the thruster value is not zero
if not thrusterVals[i] == 0:
#thrusterVals[i] += 2*random.random()-1 #random float between -1 to 1 exclusive
msg.data = thrusterVals[i]
else:
msg.data = thrusterVals[i]
thrusterPubs[i].publish(msg)
rospy.logdebug("Thrusters values published: " + str(thrusterVals))
#publish sensor hat data
tempMsg = Temperature()
#functions to give realistic sensor data curves over time
timeNowMin = rospy.get_time() / 60 ##Current ROS time in minutes
tempMsg.temperature = (
(60 * timeNowMin + 30) /
(timeNowMin + 1) - 2) + (0.5 * random.random() - 0.25)
humidMsg = RelativeHumidity()
humidMsg.relative_humidity = (
(60 * timeNowMin + 90) /
(timeNowMin + 1) - 25) + (0.5 * random.random() - 0.25)
presMsg = FluidPressure()
presMsg.fluid_pressure = (3000 * timeNowMin / (timeNowMin + 1) +
101325) + (6 * random.random() - 3)
sensorTempPub.publish(tempMsg)
sensorHumidPub.publish(humidMsg)
sensorPresPub.publish(presMsg)
rate.sleep()
def height(self, data):
pressure = FluidPressure()
pressure.fluid_pressure = (95460 - data.fluid_pressure) / 11.4
self.altitude = pressure.fluid_pressure
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)
current_odometry = Odometry()
current_key = None
print msg
while True:
get_odom_from_key()
bottom_tracking = BottomTracking()
bottom_tracking.velocity[0] = x
bottom_tracking.velocity[1] = y
bottom_tracking.velocity[2] = 0
pressure = FluidPressure()
# Saunder-Fofonoff equation
surface_pressure = 101325
ge = 9.80
rho_water = 1000
pressure.fluid_pressure = z * (rho_water * ge) + surface_pressure
imu = Imu()
quaternion = euler_to_quat(0, 0, th)
imu.orientation.x = quaternion[0]
imu.orientation.y = quaternion[1]
imu.orientation.z = quaternion[2]
imu.orientation.w = quaternion[3]
imu_pub.publish(imu)
dvl_pub.publish(bottom_tracking)
baro_pub.publish(pressure)
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()
def publish_data(self):
rate = rospy.Rate(self.FREQUENCY)
while not rospy.is_shutdown():
effort = []
thrust = []
effort.append(self.thruster_T1)
effort.append(self.thruster_T2)
effort.append(self.thruster_T3)
effort.append(self.thruster_T4)
effort.append(self.thruster_T5)
effort.append(self.thruster_T6)
effort.append(self.thruster_T7)
effort.append(self.thruster_T8)
for index in range(len(effort)):
thrust.append(self.effort_to_thrust(effort[index]))
yaw_thrust = (thrust[3] + thrust[1] - thrust[0] - thrust[2])
self.current_yaw += yaw_thrust * (1.0 / self.FREQUENCY) * 0.1
self.current_yaw %= 360.0
pitch_thrust = sub_thruster_distance*thrust[4] - sub_thruster_distance*thrust[5] - \
sub_thruster_distance*thrust[6] + sub_thruster_distance*thrust[7]
pitch_acceleration = self.thrust_to_acceleration_yaw(pitch_thrust)
self.pitch_velocity = self.acceleration_to_velocity(
pitch_acceleration, 1.0 / self.FREQUENCY, self.pitch_velocity)
self.pitch_angle = self.velocity_to_angle(self.pitch_velocity,
1.0 / self.FREQUENCY,
self.pitch_angle)
qx = quaternion_about_axis(self.current_yaw, self.zaxis)
x_thrust = (thrust[0] - thrust[1] - thrust[2] + thrust[3]) * 0.7
self.x_velocity = x_thrust * (1.0 / self.FREQUENCY) * 1.3
y_thrust = (thrust[2] + thrust[3] - thrust[0] - thrust[1]) * 0.7
self.y_velocity = y_thrust * (1.0 / self.FREQUENCY) * 1.3
#front_vector_rot = numpy.nan_to_num(qv_mult(q, self.front_vector))
#heading_vector_rot = numpy.nan_to_num(qv_mult(q, self.heading_vector))
#x_vel = self.x_velocity * (front_vector_rot[0] + heading_vector_rot[0])
#y_vel = self.y_velocity * (front_vector_rot[1] + heading_vector_rot[1])
z_thrust = thrust[4] + thrust[5] + thrust[6] + thrust[7]
self.bar += z_thrust * -0.000055
#self.z_velocity = self.acceleration_to_velocity(z_acceleration, 1.0/self.FREQUENCY, self.z_velocity)
#self.depth = self.velocity_to_depth(self.z_velocity, 1.0/self.FREQUENCY, self.depth)
#self.bar = self.depth_to_bar(self.depth, self.bar)
dvl_pressure = FluidPressure()
dvl_pressure.fluid_pressure = self.bar
self.publisher_dvl_pressure.publish(dvl_pressure)
imu = Imu()
imu.orientation.x = qx[0]
imu.orientation.y = qx[1]
imu.orientation.z = qx[2]
imu.orientation.w = qx[3]
imu.angular_velocity.y = self.pitch_velocity
imu.angular_velocity.z = self.yaw_velocity
imu.header.frame_id = "NED"
self.publisher_imu.publish(imu)
dvl_twist = TwistStamped()
dvl_twist.header.stamp = rospy.get_rostime()
dvl_twist.header.frame_id = "BODY"
dvl_twist.twist.linear.x = self.x_velocity
dvl_twist.twist.linear.y = self.y_velocity
dvl_twist.twist.linear.z = self.z_velocity
self.publisher_dvl_twist.publish(dvl_twist)
rate.sleep()
current_odometry = Odometry()
current_key = None
print msg
while True:
get_odom_from_key()
bottom_tracking = BottomTracking()
bottom_tracking.velocity[0] = x
bottom_tracking.velocity[1] = y
bottom_tracking.velocity[2] = 0
pressure = FluidPressure()
# Saunder-Fofonoff equation
surface_pressure = 101325
ge = 9.80
rho_water = 1000
pressure.fluid_pressure = z * (rho_water * ge) + surface_pressure
imu = Imu()
quaternion = euler_to_quat(0, 0, th)
imu.orientation.x = quaternion[0]
imu.orientation.y = quaternion[1]
imu.orientation.z = quaternion[2]
imu.orientation.w = quaternion[3]
imu_pub.publish(imu)
dvl_pub.publish(bottom_tracking)
baro_pub.publish(pressure)
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)
if pressure_data.read():
pressure_message.header.stamp = rospy.get_rostime()
pressure_message.fluid_pressure = pressure_data.pressure(
ms5837.UNITS_atm)
temp_message.header.stamp = rospy.get_rostime()
temp_message.temperature = pressure_data.temperature()
pub_pressure.publish(pressure_message)
pub_temp.publish(temp_message)
else:
print("Sensor read failed!")
rospy.roswarn("Failed to read pressure sensor !")
exit(1)
data = sensor3.get_distance_simple()
if data:
print("Distance: %s\tConfidence: %s%%" %
(data["distance"], data["confidence"]))
sensor = data.get("distance")
pub1.publish(sensor)
time.sleep(REFRESH_RATE_SECOND)
teleop.Update()
time_since_up += REFRESH_RATE_SECOND
bottom_tracking = BottomTracking()
bottom_tracking.velocity[0] = teleop.position[0]
bottom_tracking.velocity[1] = teleop.position[1]
bottom_tracking.velocity[2] = 0
bottom_tracking.time = time_since_up * 1000 * 1000
pressure = FluidPressure()
# Saunder-Fofonoff equation
surface_pressure = 101325
ge = 9.80
rho_water = 1000
pressure.fluid_pressure = teleop.position[2] * (rho_water *
ge) + surface_pressure
imu = Imu()
quaternion = euler_math.euler_to_quat(0, 0, teleop.orientation[2])
imu.orientation.x = quaternion[0]
imu.orientation.y = quaternion[1]
imu.orientation.z = quaternion[2]
imu.orientation.w = quaternion[3]
imu_pub.publish(imu)
dvl_pub.publish(bottom_tracking)
baro_pub.publish(pressure)
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
# publish message
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)
depth_offset = rospy.get_param('depth_offset', 0.0)
r = rospy.Rate(10) # 10hz
count = 0
while not rospy.is_shutdown():
if sensor.read():
pressure = sensor.pressure()
temp = sensor.temperature()
depth = sensor.depth()
depth = depth - depth_offset
pressure_msg = FluidPressure()
pressure_msg.header.frame_id = 'pressure_sensor'
pressure_msg.header.seq = count
pressure_msg.header.stamp = rospy.Time.now()
pressure_msg.fluid_pressure = pressure
fluid_pub.publish(pressure_msg)
depth_stamped_msg = Vector3Stamped()
depth_stamped_msg.header.frame_id = 'pressure_sensor'
depth_stamped_msg.header.seq = count
depth_stamped_msg.header.stamp = rospy.Time.now() #TODO fix this
depth_stamped_msg.vector.z = depth # TODO this
depth_stamped_pub.publish(depth_stamped_msg)
depth_msg = Float64()
depth_msg.data = depth
depth_pub.publish(depth_msg)
temp_msg = Temperature()
temp_msg.header.frame_id = 'pressure_sensor'
temp_msg = Temperature()
pres_msg = FluidPressure()
pose_msg = PoseWithCovarianceStamped()
pose_msg.pose.covariance[14] = 0.2
temp_pub = rospy.Publisher('temperature', Temperature, queue_size=1)
pres_pub = rospy.Publisher('pressure', FluidPressure, queue_size=1)
pose_pub = rospy.Publisher('depth', PoseWithCovarianceStamped, queue_size=1)
# init density, used for depth
sensor.setFluidDensity(1000)
# Spew readings
while not rospy.is_shutdown():
if sensor.read():
temp_msg.header.stamp = pres_msg.header.stamp = pose_msg.header.stamp = rospy.Time.now(
)
temp_msg.temperature = sensor.temperature() # centigrades
pres_msg.fluid_pressure = sensor.pressure() # mbar
pose_msg.pose.pose.position.z = sensor.depth() # meters
temp_pub.publish(temp_msg)
pres_pub.publish(pres_msg)
pose_pub.publish(pose_msg)
rospy.sleep(0.1)
else:
rospy.logwarn("Sensor read failed!")