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 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 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 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, i2c=None):
super().__init__('rtf_lps22')
if i2c is None:
self.i2c = busio.I2C(board.SCL, board.SDA)
else:
self.i2c = i2c
self.lps = adafruit_lps2x.LPS22(self.i2c)
self.timer_temp = self.create_timer(1.0, self.callback)
self.pub_temp = self.create_publisher(Temperature, 'temp', 10)
self.pub_pressure = self.create_publisher(FluidPressure, 'pressure',
10)
frame_id = self.declare_parameter('frame_id', "base_imu_link").value
self.temp_msg = Temperature()
self.temp_msg.header.frame_id = frame_id
self.temp_msg.variance = 0.01
self.pressure_msg = FluidPressure()
self.pressure_msg.header.frame_id = frame_id
self.pressure_msg.variance = 0.01
def reset_vars(self):
self.imu_msg = Imu()
self.imu_msg.orientation_covariance = (-1., ) * 9
self.imu_msg.angular_velocity_covariance = (-1., ) * 9
self.imu_msg.linear_acceleration_covariance = (-1., ) * 9
self.pub_imu = False
self.pub_syncout_timeref = False
self.raw_gps_msg = NavSatFix()
self.pub_raw_gps = False
self.pos_gps_msg = NavSatFix()
self.pub_pos_gps = False
self.vel_msg = TwistStamped()
self.pub_vel = False
self.mag_msg = MagneticField()
self.mag_msg.magnetic_field_covariance = (0, ) * 9
self.pub_mag = False
self.temp_msg = Temperature()
self.temp_msg.variance = 0.
self.pub_temp = False
self.press_msg = FluidPressure()
self.press_msg.variance = 0.
self.pub_press = False
self.anin1_msg = UInt16()
self.pub_anin1 = False
self.anin2_msg = UInt16()
self.pub_anin2 = False
self.ecef_msg = PointStamped()
self.pub_ecef = False
self.pub_diag = False
def read_pressure(mav_obj):
"""
Read accelerometer readings until taxis is exhausted.
There will only be output once the total time has elapsed.
"""
pub = rospy.Publisher('/depth', FluidPressure, queue_size=10)
rospy.init_node('externalpressure')
rate = rospy.Rate(10)
msg_type = 'SCALED_PRESSURE2'
msg = mav_obj.recv_match(blocking=True)
# flush out old data
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
while not rospy.is_shutdown():
msg = mav_obj.recv_match(type=msg_type, blocking=True)
h = Header()
h.stamp = rospy.Time.now()
depth_m = (msg.press_abs - 1014.25) / 100
fp = FluidPressure(header=h, fluid_pressure=depth_m, variance=0)
pub.publish(fp)
rate.sleep()
def main():
sensor = TempHumPress()
rospy.init_node("temperature_sensor")
pub_temperature = rospy.Publisher("~temperature",
Temperature,
queue_size=10)
pub_humidity = rospy.Publisher("~humidity",
RelativeHumidity,
queue_size=10)
pub_pressure = rospy.Publisher("~pressure", FluidPressure, queue_size=10)
rate = rospy.Rate(rospy.get_param('~hz', 2))
while not rospy.is_shutdown():
hdr = Header(frame_id="temperature", stamp=rospy.Time.now())
msg_temp = Temperature(header=hdr,
temperature=sensor.get_temperature_celsius())
pub_temperature.publish(msg_temp)
msg_hum = RelativeHumidity(header=hdr,
relative_humidity=sensor.get_humidity() /
100.0)
pub_humidity.publish(msg_hum)
msg_press = FluidPressure(header=hdr,
fluid_pressure=sensor.get_pressure())
pub_pressure.publish(msg_press)
rate.sleep()
def talker():
humidity_pub = rospy.Publisher('cassiopeia/environmental/humidity',
RelativeHumidity,
queue_size=10)
temperature_pub = rospy.Publisher('cassiopeia/environmental/temperature',
Temperature,
queue_size=10)
pressure_pub = rospy.Publisher('cassiopeia/environmental/pressure',
FluidPressure,
queue_size=10)
altitude_pub = rospy.Publisher('cassiopeia/altitude/',
Altitude,
queue_size=10)
rospy.init_node('environmental_sensor', anonymous=False)
rate = rospy.Rate(
1) # Max rate of BME280 sensor in normal mode in theory is 13.51 Hz
while not rospy.is_shutdown():
h = Header()
h.stamp = rospy.Time.now()
humidity_pub.publish(
RelativeHumidity(
relative_humidity=environmental_sensor.get_humidity(),
header=h))
pressure_pub.publish(
FluidPressure(fluid_pressure=environmental_sensor.get_pressure(),
header=h))
temperature_pub.publish(
Temperature(temperature=environmental_sensor.get_temperature(),
header=h))
altitude_pub.publish(
Altitude(altitude=environmental_sensor.get_altitude(), header=h))
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 __init__(self):
super().__init__('rtf_dps310')
rate = 1.0
self.timer = self.create_timer(1.0 / rate, self.callback)
self.pub_temp = self.create_publisher(Temperature, 'temperature', 10)
self.pub_pressure = self.create_publisher(FluidPressure, 'pressure',
10)
frame_id = self.declare_parameter('frame_id', "base_imu_link").value
self.temp_msg = Temperature()
self.temp_msg.header.frame_id = frame_id
self.temp_msg.variance = 0.01
self.pressure_msg = FluidPressure()
self.pressure_msg.header.frame_id = frame_id
self.pressure_msg.variance = 0.01
def __init__(self):
super().__init__('mock_publisher')
# TODO: attributes can jump to values not related to the past one,
# make it in increments
self._to_publish = [
(self.create_publisher(Pose2D, '/coordinates', 10),
lambda: Pose2D(x=float(randint(-100, 100)),
y=float(randint(-100, 100)),
theta=random() * 2 * pi)),
(self.create_publisher(FluidPressure, '/environment/pressure', 10),
lambda: FluidPressure(fluid_pressure=float(randint(30000, 70000)))
),
(self.create_publisher(Temperature, '/environment/temperature',
10),
lambda: Temperature(temperature=float(randint(0, 60)))),
(self.create_publisher(Float32, '/leds',
10), lambda: Float32(data=random())),
]
timer_period = 2
self.timer = self.create_timer(timer_period, self.timer_callback)
def baro():
rate = rospy.Rate(10)
baro = navio.ms5611.MS5611()
baro.initialize()
pressure_pub = rospy.Publisher("baro/pressure", FluidPressure)
temp_pub = rospy.Publisher("baro/temp", Temperature)
ringbuf_size = rospy.get_param("~moving_avg_size", 50)
presbuffer = RingBuffer(ringbuf_size)
rospy.loginfo("Starting barometer")
while not rospy.is_shutdown():
baro.refreshPressure()
rospy.sleep(0.01) # wait 10ms per Navio docs
baro.readPressure()
baro.refreshTemperature()
rospy.sleep(0.01) # wait 10ms per Navio docs
baro.readTemperature()
baro.calculatePressureAndTemperature()
temp = baro.TEMP
pressure = baro.PRES * 100 # convert millibars to Pascals
presbuffer.append(pressure)
pressurelist = presbuffer.get()
# average it out
avg_pressure = sum(pressurelist) / len(pressurelist)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "map"
pressure_pub.publish(FluidPressure(header, avg_pressure, 0))
temp_pub.publish(Temperature(header, temp, 0))
rate.sleep()
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 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 __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.02)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"
% (device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
# optional no rotation procedure for internal calibration of biases
# (only mark iv devices)
no_rotation_duration = get_param('~no_rotation_duration', 0)
if no_rotation_duration:
rospy.loginfo("Starting the no-rotation procedure to estimate the "
"gyroscope biases for %d s. Please don't move the IMU"
" during this time." % no_rotation_duration)
self.mt.SetNoRotation(no_rotation_duration)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.pos_pub = None
self.gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu/imu_data_str', String, queue_size=10)
# predefinition of used variables
self.imu_msg = Imu()
self.imu_msg_old = Imu()
self.pos_msg = NavSatFix()
self.pos_msg_old = NavSatFix()
self.gps_msg = NavSatFix()
self.gps_msg_old = NavSatFix()
self.vel_msg = TwistStamped()
self.vel_msg_old = TwistStamped()
self.mag_msg = MagneticField()
self.mag_msg_old = MagneticField()
self.temp_msg = Temperature()
self.temp_msg_old = Temperature()
self.press_msg = FluidPressure()
self.press_msg_old = FluidPressure()
self.anin1_msg = UInt16()
self.anin1_msg_old = UInt16()
self.anin2_msg = UInt16()
self.anin2_msg_old = UInt16()
self.ecef_msg = PointStamped()
self.ecef_msg_old = PointStamped()
# triggers for new msg to publish
self.pub_imu = False
self.pub_pos = False
self.pub_gps = False
self.pub_vel = False
self.pub_mag = False
self.pub_temp = False
self.pub_press = False
self.pub_anin1 = False
self.pub_anin2 = False
self.pub_ecef = False
self.pub_diag = False
th = 0
status = 0
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)
0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02
]
# 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
class Ulink:
msg = dict()
CONT = dict()
imuMsg = Imu()
poseMsg = PoseWithCovarianceStamped()
pose_navMsg = PoseWithCovarianceStamped()
compassMsg = MagneticField()
remoteMsg = Joy()
baroMsg = FluidPressure()
tempMsg = Temperature()
#rospy.init_node('imu_node2', anonymous=True)
time_start = rospy.Time.now()
imuMsg.orientation_covariance = [0.005, 0, 0, 0, 0.005, 0, 0, 0, 0.005]
imuMsg.angular_velocity_covariance = [
1.2184696791468346e-7, 0, 0, 0, 1.2184696791468346e-7, 0, 0, 0,
1.2184696791468346e-7
]
imuMsg.linear_acceleration_covariance = [
8.99999999999e-8, 0, 0, 0, 8.99999999999e-8, 0, 0, 0, 8.99999999999e-8
]
#u = serial.Serial()
def __init__(self, ser):
try:
self.s = ser
print "Conecting.."
except ValueError:
print "Cannot connect the Port"
if self.s.isOpen():
print "Conected"
time.sleep(2)
## self.u.write('X')
## print 'wait start..'
## while (ord(self.u.read(1)) != 0xa1) :
## time.sleep(0.1)
## 'wait start cmd'
## time.sleep(0.2)
print "sending data request"
self.send_data_request(self.s, DATA_ATT)
self.send_data_request(self.s, DATA_GPS)
print "Starting message handler thread"
self.msg['STATUS'] = 'alive'
thread.start_new_thread(self.MsgHandler, (self.s, ))
#thread.start_new_thread(self.cmd_send,())
#self.send_cmd_takeoff(5)
## for i in range (0,10) :
## print self.u.write(chr(0xaa))
def MsgHandler(self, s):
while s.isOpen():
if s.inWaiting() > 0:
ch = s.read(1)
if ord(ch) == 0xb5:
hdr = ord(ch)
lenp = ord(s.read(1))
idp = ord(s.read(1))
pay = s.read(lenp)
chk = struct.unpack('h', s.read(2))[0]
summ = 0
for c in pay:
summ = summ + ord(c)
if chk == hdr + lenp + idp + summ:
#print 'chk ok'
#print idp,lenp
self.packet_decode(idp, pay, lenp)
def wait_alive(self, s):
'nothing'
def send_data_request(self, s, d):
#hdr =0xb5 len =1 id = 6
chk = 181 + 1 + DATA_REQUEST + d
data = struct.pack('cccch', chr(181), chr(1), chr(DATA_REQUEST),
chr(d), chk)
s.write(data)
def send_cmd_takeoff(self, h):
chk = 181 + 1 + CMD_TAKEOFF + h
return self.s.write(
struct.pack('cccch', chr(181), chr(1), chr(CMD_TAKEOFF), chr(h),
chk))
def send_position_desire(self, px, py, pz, yaw):
dlen = 16
head = struct.pack('ccc', chr(181), chr(dlen), chr(DATA_MOTOR))
data = struct.pack('ffff', px, py, pz, yaw)
summ = 181 + dlen + DATA_MOTOR
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def packet_decode(self, msgid, payload, lenp):
#print 'get ',msgid
if msgid == ATTITUDE:
#print 'id : ' ,idp
d = struct.unpack('fff', payload)
#d=struct.unpack('iii',payload)
self.msg.update({'ATTITUDE': d})
q = tf.transformations.quaternion_from_euler(
d[0], d[1], d[2] + 1.57079632679
) #roll pitch yaw (addition for use in rviz +p/2)
self.imuMsg.header.stamp = rospy.Time.now() - self.time_start
self.imuMsg.orientation.x = q[0]
self.imuMsg.orientation.y = q[1]
self.imuMsg.orientation.z = q[2]
self.imuMsg.orientation.w = q[3]
self.poseMsg.header.stamp = rospy.Time.now() - self.time_start
self.poseMsg.pose.pose.orientation.x = self.imuMsg.orientation.x
self.poseMsg.pose.pose.orientation.y = self.imuMsg.orientation.y
self.poseMsg.pose.pose.orientation.z = self.imuMsg.orientation.z
self.poseMsg.pose.pose.orientation.w = self.imuMsg.orientation.w
#self.imuMsg.linear_acceleration.x = d[0]
#self.imuMsg.linear_acceleration.y = d[1]
#self.imuMsg.linear_acceleration.z = d[2]
#print 'ATT ',d[0],d[1],d[2]
# print 'ATTITUDE roll', str(d[0]*180/3.14)[:5] ,' pitch :' , str(d[1]*180/3.14)[:5] ,' yaw:' , str(d[2]*180/3.14)[:6]
elif msgid == RAW_IMU:
d = struct.unpack('fffffffff', payload)
self.msg.update({'RAW_IMU': d})
self.imuMsg.linear_acceleration.x = d[0] * 9.80655
self.imuMsg.linear_acceleration.y = d[1] * 9.80655
self.imuMsg.linear_acceleration.z = d[2] * 9.80655
self.imuMsg.angular_velocity.x = d[3]
self.imuMsg.angular_velocity.y = d[4]
self.imuMsg.angular_velocity.z = d[5]
self.compassMsg.header.stamp = rospy.Time.now() - self.time_start
self.compassMsg.magnetic_field.x = d[6]
self.compassMsg.magnetic_field.y = d[7]
self.compassMsg.magnetic_field.z = d[8]
elif msgid == ALTITUDE:
d = struct.unpack('ff', payload)
self.msg.update({'ALTITUDE': d})
self.poseMsg.pose.pose.position.z = d[0]
self.pose_navMsg.pose.pose.position.z = d[0]
# print 'ALT rel_ALT : ', str(d[0]*100)[:4] ,' vel_z :' , str(d[1]*10)[:4]
elif msgid == RAW_BARO:
d = struct.unpack('ff', payload) #pressure , temperature
self.msg.update({'RAW_BARO': d})
self.baroMsg.fluid_pressure = d[0]
self.tempMsg.temperature = d[1]
elif msgid == RAW_RADIO:
d = struct.unpack('HHHHHH', payload)
self.msg.update({'RAW_RADIO': d})
self.remoteMsg.header.stamp = rospy.Time.now() - self.time_start
# self.remoteMsg.pose.position.x = d[0]
# self.remoteMsg.pose.position.y = d[1]
# self.remoteMsg.pose.position.z = d[2]
# self.remoteMsg.pose.orientation.x = d[3]
# self.remoteMsg.pose.orientation.y = d[4]
# self.remoteMsg.pose.orientation.z = d[5]
self.remoteMsg.axes = d[0:5]
self.send_position_desire(d[2] / 8, d[2] / 8, d[2] / 8, d[2] / 8)
#print 'r1:', str(d[0])[:4] ,'r2:' , str(d[1])[:4] ,'r3:' , str(d[2])[:4] ,'r4:' , str(d[3])[:4] ,'r5:' , str(d[4])[:4] ,'r6:' , str(d[5])[:4]
elif msgid == MOTOR_OUT:
d = struct.unpack('hhhh', payload)
self.msg.update({'MOTOR_OUT': d})
elif msgid == CNT_VAL:
d = struct.unpack('fff', payload)
self.msg.update({'CNT_VAL': d})
# print 'CONTROL ROLL : ', str(d[0])[:4] ,' PITCH :' , str(d[1])[:4] ,' YAW :' , str(d[2])[:4]
elif msgid == STATUS_CHK:
d = struct.unpack('h', payload)
self.msg.update({'STATUS_CHK': d})
print 'NUM SEN : ', str(d[0])
elif msgid == UNKNOWN_TYPE:
print struct.unpack('%ds' % lenp, payload)
#self.msg.update({'UNKOWN_TYPE': s})
elif msgid == UNKNOWN_FLOAT:
d = struct.unpack('%df' % (lenp / 4), payload)
## a=(d)
## for i in range((lenp/4)) :
## a[i] = (round(a[i],5))
## print a
## #self.msg.update({'UNKOWN_TYPE': s})
#print round(d[0]*100,5),'\t',round(d[1]*100,5),'\t',round(d[2]*100,5),'\t',round(d[3]*100,5),'\t',round(d[4]*100,5),'\t',round(d[5]*100,5),'\t',round(d[6]*100,5),'\t',round(d[7]*100,5),'\t',round(d[8],5)
elif msgid == GPS:
#print 'get gps'
gps = struct.unpack('hh', payload)
#self.msg.update({'GPS': (gps[0]/(1e7),gps[1]/(1e7))})
self.msg.update({'GPS': (gps[0], gps[1])})
#print 'GPS lat', gps[0] ,' lon :' , gps[1]
self.poseMsg.pose.pose.position.x = gps[1] #CM
self.poseMsg.pose.pose.position.y = gps[0]
elif msgid == NAV_DATA:
d = struct.unpack('hh', payload)
self.msg.update({'NAV_DATA': d})
self.pose_navMsg.pose.pose.position.x = d[0] #CM
self.pose_navMsg.pose.pose.position.y = d[1]
#print 'nav x:', d[0] ,'nav y:' , d[1]
elif msgid == CONT_COMPASS:
compass = struct.unpack('hhhhhh', payload)
self.CONT.update({'COMPASS': compass})
print 'Update Compass constance..'
elif msgid == CONT_PID:
pid = struct.unpack('HHHHHHHHHHHH', payload)
self.CONT.update({'PID': pid})
print 'Update PID constance..'
def takeoff(self, h):
return self.send_cmd_takeoff(h)
def msg_print(self):
print '---------------------------------------------'
print self.msg
print '---------------------------------------------'
def get_msg(self):
return self.msg
def get_cont(self):
return self.CONT
def getATT(self):
if 'ATTITUDE' in self.msg:
return self.msg['ATTITUDE'] #roll,pitch,yaw
else:
return None
def getALT(self):
if 'ALTITUDE' in self.msg:
return self.msg['ALTITUDE'] #rel_alt,vel_z
else:
return None
def getCNT_VAL(self):
if 'CNT_VAL' in self.msg:
return self.msg['CNT_VAL'] #control roll pitch yaw
else:
return None
def getRAW_RADIO(self):
if 'RAW_RADIO' in self.msg:
return self.msg['RAW_RADIO'] #control roll pitch yaw
else:
return None
def getGPS(self):
if 'GPS' in self.msg:
return self.msg['GPS']
else:
return None
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()
th = 0
status = 0
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)
class Ulink:
msg = dict()
CONT = dict()
imuMsg = Imu()
poseMsg = PoseWithCovarianceStamped()
navvelMsg = TwistWithCovarianceStamped()
pose_navMsg = Odometry()
altMsg = Odometry()
compassMsg = MagneticField()
remoteMsg = Joy()
baroMsg = FluidPressure()
tempMsg = Temperature()
gpsrawMsg = NavSatFix()
desire_navMSG = Odometry()
ackMsg = Ack()
sonarMsg = Range()
NavMsg = Navdata()
#rospy.init_node('imu_node2', anonymous=True)
time_start = rospy.Time.now()
imuMsg.orientation_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1]
imuMsg.angular_velocity_covariance = [
1.2184696791468346e-7, 0, 0, 0, 1.2184696791468346e-7, 0, 0, 0,
1.2184696791468346e-7
]
imuMsg.linear_acceleration_covariance = [
8.99999999999e-8, 0, 0, 0, 8.99999999999e-8, 0, 0, 0, 8.99999999999e-8
]
#u = serial.Serial()
def __init__(self, ser):
try:
self.s = ser
print "Conecting.."
except ValueError:
print "Cannot connect the Port"
if self.s.isOpen():
print "Conected"
time.sleep(2)
## self.u.write('X')
## print 'wait start..'
## while (ord(self.u.read(1)) != 0xa1) :
## time.sleep(0.1)
## 'wait start cmd'
## time.sleep(0.2)
#print "sending data request"
#self.send_data_request(self.s,DATA_ATT)
#self.send_data_request(self.s,DATA_GPS)
print "Starting message handler thread"
self.msg['STATUS'] = 'alive'
thread.start_new_thread(self.MsgHandler, (self.s, ))
#thread.start_new_thread(self.cmd_send,())
#self.send_cmd_takeoff(5)
## for i in range (0,10) :
## print self.u.write(chr(0xaa))
def MsgHandler(self, s):
while s.isOpen():
if s.inWaiting() > 0:
ch = s.read(1)
#print ord(ch)
if ord(ch) == 0xb5:
hdr = ord(ch)
lenp = ord(s.read(1))
idp = ord(s.read(1))
pay = s.read(lenp)
chk = struct.unpack('h', s.read(2))[0]
summ = 0
for c in pay:
summ = summ + ord(c)
if chk == hdr + lenp + idp + summ:
#print 'chk ok'
#print idp,lenp
self.packet_decode(idp, pay, lenp)
def wait_alive(self, s):
'nothing'
def send_data_request(self, s, d):
#hdr =0xb5 len =1 id = 6
chk = 181 + 1 + DATA_REQUEST + d
data = struct.pack('cccch', chr(181), chr(1), chr(DATA_REQUEST),
chr(d), chk)
s.write(data)
def send_cmd_takeoff(self, h):
chk = 181 + 1 + CMD_TAKEOFF + h
return self.s.write(
struct.pack('cccch', chr(181), chr(1), chr(CMD_TAKEOFF), chr(h),
chk))
def send_position_desire(self, px, py, pz, yaw):
dlen = 16
head = struct.pack('ccc', chr(181), chr(dlen), chr(POSE_DEMAND))
data = struct.pack('ffff', px, py, pz, yaw)
summ = 181 + dlen + POSE_DEMAND
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_flow(self, fx, fy, track):
dlen = 10
head = struct.pack('ccc', chr(181), chr(dlen), chr(FLOW_DATA))
data = struct.pack('ffH', fx, fy, track)
summ = 181 + dlen + FLOW_DATA
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_motor_desire(self, M1, M2, M3, M4):
dlen = 16
head = struct.pack('ccc', chr(181), chr(dlen), chr(MOTOR_OUT))
data = struct.pack('ffff', M1, M2, M3, M4)
summ = 181 + dlen + MOTOR_OUT
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def packet_decode(self, msgid, payload, lenp):
#print 'get ',msgid
t_now = rospy.Time.now()
if msgid == ATTITUDE:
#print 'id : ' ,idp
d = struct.unpack('hhh', payload)
#d=struct.unpack('iii',payload)
self.msg.update({'ATTITUDE': d})
self.NavMsg.tm = 1000000 * (t_now.secs - self.time_start.secs) + (
t_now.nsecs - self.time_start.nsecs) / 1000
self.NavMsg.rotX = d[0] * 0.0572957
self.NavMsg.rotY = d[1] * 0.0572957
self.NavMsg.rotZ = d[2] * 0.0572957
q = tf.transformations.quaternion_from_euler(
d[0] * 0.001, d[1] * 0.001, d[2] *
0.001) #roll pitch yaw (addition for use in rviz +p/2)
self.imuMsg.header.stamp = t_now #-self.time_start
self.imuMsg.orientation.x = q[0]
self.imuMsg.orientation.y = q[1]
self.imuMsg.orientation.z = q[2]
self.imuMsg.orientation.w = q[3]
self.poseMsg.pose.pose.orientation.x = self.imuMsg.orientation.x
self.poseMsg.pose.pose.orientation.y = self.imuMsg.orientation.y
self.poseMsg.pose.pose.orientation.z = self.imuMsg.orientation.z
self.poseMsg.pose.pose.orientation.w = self.imuMsg.orientation.w
#self.imuMsg.linear_acceleration.x = d[0]
#self.imuMsg.linear_acceleration.y = d[1]
#self.imuMsg.linear_acceleration.z = d[2]
#print 'ATT ',d[0],d[1],d[2]
# print 'ATTITUDE roll', str(d[0]*180/3.14)[:5] ,' pitch :' , str(d[1]*180/3.14)[:5] ,' yaw:' , str(d[2]*180/3.14)[:6]
elif msgid == RAW_IMU:
d = struct.unpack('hhhhhh', payload)
self.msg.update({'RAW_IMU': d})
self.imuMsg.header.stamp = t_now
self.imuMsg.linear_acceleration.x = d[0] * 9.80655 / 1000.0
self.imuMsg.linear_acceleration.y = d[1] * 9.80655 / 1000.0
self.imuMsg.linear_acceleration.z = d[2] * 9.80655 / 1000.0
self.imuMsg.angular_velocity.x = d[3] / 1000.0
self.imuMsg.angular_velocity.y = d[4] / 1000.0
self.imuMsg.angular_velocity.z = d[5] / 1000.0
self.compassMsg.header.stamp = t_now #-self.time_start
# self.compassMsg.magnetic_field.x = d[6]
# self.compassMsg.magnetic_field.y = d[7]
# self.compassMsg.magnetic_field.z = d[8]
self.NavMsg.ax = d[0] / 1000.00
self.NavMsg.ay = d[1] / 1000.00
self.NavMsg.az = d[2] / 1000.00
elif msgid == V_VAL:
d = struct.unpack('hh', payload) #v_est_x v_est_y
self.msg.update({'V_VAL': d})
self.pose_navMsg.twist.twist.linear.x = d[0] * 0.01
self.pose_navMsg.twist.twist.linear.y = d[1] * 0.01
#print 'r1:', str(d[0])[:4] ,'r2:' , str(d[1])[:4]
elif msgid == RAW_BARO:
d = struct.unpack('fff',
payload) #alt_bybaro , pressure , temperature
global baro_old
global temp_old
self.msg.update({'RAW_BARO': d})
# if baro_old == 0 or temp_old ==0:
# baro_old = d[1]
# temp_old = d[2]
# else :
# if abs(d[1]-baro_old)>2 or abs(d[2]-temp_old)>1:
# self.poseMsg.pose.pose.position.x=1+self.poseMsg.pose.pose.position.x
# else :
# self.baroMsg.fluid_pressure = d[1]
# self.tempMsg.temperature = d[2]
# baro_old = d[1]
# temp_old = d[2]
#print 'BARODATA : ', str(d[1])[:7] ,' temp :' , str(d[2])[:7] ,' alt :' , str(d[0])[:7]
elif msgid == RAW_RADIO:
d = struct.unpack('HHHHHH', payload)
self.msg.update({'RAW_RADIO': d})
self.remoteMsg.header.stamp = t_now #-self.time_start
# self.remoteMsg.pose.position.x = d[0]
# self.remoteMsg.pose.position.y = d[1]
# self.remoteMsg.pose.position.z = d[2]
# self.remoteMsg.pose.orientation.x = d[3]
# self.remoteMsg.pose.orientation.y = d[4]
# self.remoteMsg.pose.orientation.z = d[5]
self.remoteMsg.axes = d[0:5]
#self.send_motor_desire(d[2]/8,d[2]/8,d[2]/8,d[2]/8)
#print 'r1:', str(d[0])[:4] ,'r2:' , str(d[1])[:4] ,'r3:' , str(d[2])[:4] ,'r4:' , str(d[3])[:4] ,'r5:' , str(d[4])[:4] ,'r6:' , str(d[5])[:4]
elif msgid == MOTOR_OUT:
d = struct.unpack('hhhh', payload)
self.msg.update({'MOTOR_OUT': d})
#print str(d[0]), '\t', str(d[1]) ,'\t', str(d[2]),'\t', str(d[3])
elif msgid == CNT_VAL:
d = struct.unpack('ffff', payload)
self.msg.update({'CNT_VAL': d})
#print 'CT: ', str(d[0])[:4] ,'CR : ', str(d[1])[:4] ,' CP :' , str(d[2])[:4] ,' CY :' , str(d[3])[:4]
# elif msgid == SONAR_DATA2_O :
# d=struct.unpack('f',payload)
# self.msg.update({'SONAR_DATA2_O': d})
# self.sonarMsg.header.stamp = t_now
# self.sonarMsg.range = d[0]
# print 'SONAR : ', str(d[0])[:4]
elif msgid == STATUS_CHK:
d = struct.unpack('hhhh', payload)
self.msg.update({'STATUS_CHK': d})
print 'NUM SEN : ', str(d[0]), '--ARM : ', bool(
d[1]), '--MODE : ', modes[d[2]](), '--RC : ', d[3]
elif msgid == UNKNOWN_TYPE:
print struct.unpack('%ds' % lenp, payload)
#self.msg.update({'UNKOWN_TYPE': s})
elif msgid == UNKNOWN_FLOAT:
d = struct.unpack('%df' % (lenp / 4), payload)
## a=(d)
## for i in range((lenp/4)) :
## a[i] = (round(a[i],5))
## print a
## #self.msg.update({'UNKOWN_TYPE': s})
#print round(d[0]*100,5),'\t',round(d[1]*100,5),'\t',round(d[2]*100,5),'\t',round(d[3]*100,5)
elif msgid == GPS: # north(cm) , east (cm) , v_est(cm) , v_north(cm)
#print 'get gps'
gps = struct.unpack('ffhh', payload)
#self.msg.update({'GPS': (gps[0]/(1e7),gps[1]/(1e7))})
self.msg.update({'GPS': (gps[0], gps[1])})
#print 'GPS lat', gps[0] ,' lon :' , gps[1]
self.poseMsg.header.stamp = t_now #-self.time_start
self.poseMsg.pose.pose.position.x = gps[1] / 100.00 #to m
self.poseMsg.pose.pose.position.y = gps[0] / 100.00
#ros UTM y is northing , x is easting
self.navvelMsg.header.stamp = t_now #-self.time_start
self.navvelMsg.twist.twist.linear.x = gps[2] / 100.00
self.navvelMsg.twist.twist.linear.y = gps[3] / 100.00
elif msgid == GPS_RAW_FIX: #lat,lng , numsat, hacc ,alt,fix_type,vacc
#print 'get gps'
gps_raw = struct.unpack('iiHHiHHH', payload)
#self.msg.update({'GPS': (gps[0]/(1e7),gps[1]/(1e7))})
self.msg.update({
'GPS_RAW_FIX': (gps_raw[0], gps_raw[1], gps_raw[2], gps_raw[3],
gps_raw[4], gps_raw[5], gps_raw[6], gps_raw[7])
}) #lat lon numsat hacc alt fix vacc
#print 'GPS lat', gps[0] ,' lon :' , gps[1]
self.gpsrawMsg.header.stamp = t_now #-self.time_start
self.gpsrawMsg.latitude = gps_raw[0] / 10000000.00000000 #deg
self.gpsrawMsg.longitude = gps_raw[1] / 10000000.00000000
self.gpsrawMsg.status.status = -1 if gps_raw[5] == 0 else 1
self.gpsrawMsg.position_covariance = [
gps_raw[3] * 0.01, 0, 0, 0, gps_raw[3] * 0.01, 0, 0, 0,
gps_raw[6] * 0.01
]
self.poseMsg.pose.covariance = [
gps_raw[3] * 0.01, 0, 0, 0, 0, 0, 0, gps_raw[3] * 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1
]
#chamge covariance term z position and z_vel to covariance of baro
# [gps_raw[3], 0, 0, 0, 0, 0,
# 0, gps_raw[3],0, 0, 0, 0,
# 0, 0, gps_raw[6],0, 0, 0,
# 0, 0, 0, 0.01, 0, 0,
# 0, 0, 0, 0, 0.01, 0,
# 0, 0, 0, 0, 0, 0.1]
self.navvelMsg.twist.covariance = [
gps_raw[7] * 0.01, 0, 0, 0, 0, 0, 0, gps_raw[7] * 0.01, 0, 0,
0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0
]
self.gpsrawMsg.status.service = gps_raw[2]
self.gpsrawMsg.altitude = gps_raw[4] / 1000.00000000
self.gpsrawMsg.position_covariance_type = 2
elif msgid == NAV_DATA:
d = struct.unpack('ff', payload)
self.msg.update({'NAV_DATA': d})
self.pose_navMsg.header.stamp = t_now
self.pose_navMsg.pose.pose.position.x = d[0] #to m
self.pose_navMsg.pose.pose.position.y = d[1]
#print 'nav x:', d[0] ,'nav y:' , d[1]
elif msgid == ALTITUDE:
d = struct.unpack('fh', payload)
self.msg.update({'ALTITUDE': d})
# self.poseMsg.pose.pose.position.z = d[0]
self.pose_navMsg.pose.pose.position.z = d[0]
# self.poseMsg.header.stamp = t_now#-self.time_start
# self.poseMsg.pose.pose.position.z = d[0]
self.altMsg.header.stamp = t_now #-self.time_start
self.altMsg.pose.pose.position.z = d[0]
self.altMsg.twist.twist.linear.z = d[1] / 100.000 #to m/s
self.altMsg.pose.covariance = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.altMsg.twist.covariance = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.navvelMsg.twist.twist.linear.z = d[1] / 100.000 #to m/s
# print d[0]*1000
self.NavMsg.altd = int(d[0] * 1000)
#report in mm
# print 'ALT rel_ALT : ', str(d[0]*100)[:4] ,' vel_z :' , str(d[1]*10)[:4]
elif msgid == CONT_COMPASS:
compass = struct.unpack('hhhhhh', payload)
self.CONT.update({'COMPASS': compass})
print 'Update Compass constance..'
elif msgid == CONT_PID:
pid = struct.unpack('HHHHHHHHHHHH', payload)
self.CONT.update({'PID': pid})
print 'Update PID constance..'
elif msgid == ACK:
d = struct.unpack('H', payload)[0]
ack = []
for i in range(16):
ack.append(bool(d & (0x0001 << i)))
print ack
self.ackMsg.ACK_PENDING = ack[0]
self.ackMsg.ACK_GCONFIG = ack[1]
self.ackMsg.ACK_PID_RPY1 = ack[2]
self.ackMsg.ACK_PID_RPY2 = ack[3]
self.ackMsg.ACK_PID_ALT = ack[4]
self.ackMsg.ACK_PID_NAV = ack[5]
self.ackMsg.ACK_SPD = ack[6]
self.ackMsg.ACK_NAV_INER = ack[7]
self.ackMsg.ACK_MODE = ack[8]
#del self.ack[:]
print "....."
elif msgid == DESIRE_NAV:
d = struct.unpack('hhhhhhh', payload)
self.msg.update({'DESIRE_NAV': d})
self.desire_navMSG.header.stamp = t_now #-self.time_start
self.desire_navMSG.pose.pose.position.x = d[0] * 0.01
self.desire_navMSG.pose.pose.position.y = d[2] * 0.01
self.desire_navMSG.pose.pose.position.z = d[4] * 0.01
self.desire_navMSG.twist.twist.linear.x = d[1] * 0.01
self.desire_navMSG.twist.twist.linear.y = d[3] * 0.01
self.desire_navMSG.twist.twist.linear.z = d[5] * 0.01
q = tf.transformations.quaternion_from_euler(0, 0, d[6] * 0.01)
self.desire_navMSG.pose.pose.orientation.x = q[0]
self.desire_navMSG.pose.pose.orientation.y = q[1]
self.desire_navMSG.pose.pose.orientation.z = q[2]
self.desire_navMSG.pose.pose.orientation.w = q[3]
def takeoff(self, h):
return self.send_cmd_takeoff(h)
def msg_print(self):
print '---------------------------------------------'
print self.msg
print '---------------------------------------------'
def send_pid1_param(self, data): #(6 float)
dlen = 24
head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_RPY1))
data = struct.pack('ffffff', data[0], data[1], data[2], data[3],
data[4], data[5])
summ = 181 + dlen + PID_RPY1
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_pid2_param(self, data): #(6 float)
dlen = 24
head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_RPY2))
data = struct.pack('ffffff', data[0], data[1], data[2], data[3],
data[4], data[5])
summ = 181 + dlen + PID_RPY2
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_pid_alt_param(self, data): #(5 float)
dlen = 20
head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_ALT))
data = struct.pack('fffff', data[0], data[1], data[2], data[3],
data[4])
summ = 181 + dlen + PID_ALT
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_pid_nav_param(self, data): #(5 float)
dlen = 20
head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_NAV))
data = struct.pack('fffff', data[0], data[1], data[2], data[3],
data[4])
summ = 181 + dlen + PID_NAV
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_spd_param(self, data): #(2 float)
dlen = 8
head = struct.pack('ccc', chr(181), chr(dlen), chr(PID_SPD))
data = struct.pack('ff', data[0], data[1])
summ = 181 + dlen + PID_SPD
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_nav_iner_param(self, data): #(5 float)
dlen = 20
head = struct.pack('ccc', chr(181), chr(dlen), chr(NAV_INER))
data = struct.pack('fffff', data[0], data[1], data[2], data[3],
data[4])
summ = 181 + dlen + NAV_INER
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_mode_param(self, data): #(6 float)
dlen = 24
head = struct.pack('ccc', chr(181), chr(dlen), chr(MODE_CFG))
data = struct.pack('ffffff', data[0], data[1], data[2], data[3],
data[4], data[5])
summ = 181 + dlen + MODE_CFG
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
return tatal
def send_vision_data(self, datav): #(3 float 1 uint)
dlen = 14
head = struct.pack('ccc', chr(181), chr(dlen), chr(VISION_DATA))
data = struct.pack('fffH', datav[0], datav[1], datav[2], datav[3])
summ = 181 + dlen + VISION_DATA
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
print 'p x:', datav[0], 'p y:', datav[1]
return tatal
def send_msf_data(self, datam): #(3 float 1 uint)
dlen = 24
head = struct.pack('ccc', chr(181), chr(dlen), chr(MSF_DATA))
data = struct.pack('ffffff', datam[0], datam[1], datam[2], datam[3],
datam[4], datam[5])
summ = 181 + dlen + MSF_DATA
for d in str(data):
summ += ord(d)
chk = struct.pack('h', summ)
tatal = len(head) + len(data) + len(chk)
self.s.write(head)
self.s.write(data)
self.s.write(chk)
# print 'p x:', datam[0] ,'p y:' , datam[1]
return tatal