def main():
sense = SenseHat()
filename = 'compass.txt'
#open file for write (rewrite its content)
file = open(filename, 'w')
sense.stick.direction_middle = stop
print('Start data acquisition...')
#calibration process
while calibration:
magnet = sense.get_compass_raw()
x = magnet['x']
y = magnet['y']
file.write(str(x) + ',' + str(y) + '\n')
file.close()
xmax, xmin, ymax, ymin = plot(filename)
while True:
magnet = sense.get_compass_raw()
x = magnet['x']
y = magnet['y']
#range transform
xz = -1 + ((1 - (-1)) / (xmax - xmin)) * (x - xmin)
yz = -1 + ((1 - (-1)) / (ymax - ymin)) * (y - ymin)
#degree (a) calculation
if xz == 0 and yz < 0:
deg = 90
elif xz == 0 and yz > 0:
deg = 270
elif yz < 0:
deg = 360 + math.atan2(yz, xz) * (180 / 3.14159)
else:
deg = math.atan2(yz, xz) * (180 / 3.14159)
#points
if deg < 22.5 or deg > 337.5:
sense.show_letter('N')
elif deg < 67.5:
sense.show_message(' NE')
elif deg < 112.5:
sense.show_letter('E')
elif deg < 157.5:
sense.show_message('SE')
elif deg < 202.5:
sense.show_letter('S')
elif deg < 247.5:
sense.show_message('SW')
elif deg < 292.5:
sense.show_letter('W')
else:
sense.show_message('NW')
time.sleep(0.2)
sense.clear()
def main():
sense = SenseHat()
sense.set_imu_config(True, True, True)
gyro = sense.get_gyroscope_raw()
acc = sense.get_accelerometer_raw()
comp = sense.get_compass_raw()
temp = sense.temp
humidity = sense.humidity
pressure = sense.pressure
data = [{
"id": "gyro",
"values": [gyro['x'], gyro['y'], gyro['z']]
}, {
"id": "acc",
"values": [acc['x'], acc['y'], acc['z']]
}, {
"id": "comp",
"values": [comp['x'], comp['y'], comp['z']]
}, {
"id": "temp",
"values": [temp]
}, {
"id": "humidity",
"values": [humidity]
}, {
"id": "pressure",
"values": [pressure]
}]
print(json.dumps(data))
def get_sensors(precision):
"""
get temp, pressure, humidity from the Sense HAT
:param precision: Decimal point round precision, e.g. with 3 the results will be 24.054. Default 2
:return: returns a data dictionary
"""
sense = SenseHat()
data = {}
data['temperature'] = round(sense.get_temperature(), precision)
data['pressure'] = round(sense.get_pressure(), precision)
data['humidity'] = round(sense.get_humidity(), precision)
data['temperature_h'] = round(sense.get_temperature_from_humidity(),
precision)
data['temperature_p'] = round(sense.get_temperature_from_pressure(),
precision)
magnetometer_raw = sense.get_compass_raw()
data['magnetometer_x'] = magnetometer_raw['x']
data['magnetometer_y'] = magnetometer_raw['y']
data['magnetometer_z'] = magnetometer_raw['z']
gyroscope_raw = sense.get_gyroscope_raw()
data['gyroscope_x'] = gyroscope_raw['x']
data['gyroscope_y'] = gyroscope_raw['y']
data['gyroscope_z'] = gyroscope_raw['z']
accelerometer_raw = sense.get_accelerometer_raw()
data['accelerometer_x'] = accelerometer_raw['x']
data['accelerometer_y'] = accelerometer_raw['y']
data['accelerometer_z'] = accelerometer_raw['z']
return data
def SenseHatData(conf, dataobj):
sense = SenseHat()
sense.clear()
zero_pressure = sense.get_pressure()
while not conf.shutdown:
# Adjust ground pressure in case of anomaly
if conf.state == "HALT":
zero_pressure = zero_pressure * .9 + sense.get_pressure() * .1
# Altimeter
data = dataobj.current_data
current_pressure = sense.get_pressure()
data["sensors"]["alt"] = get_altitude(
zero_pressure, sense.get_pressure(),
sense.get_temperature()) # meters
data["sensors"]["hum"] = sense.get_humidity() # %
data["sensors"]["temp"] = (sense.get_temperature() * 9 / 5) + 32 # F
data["sensors"]["pres"] = current_pressure
conf.data.add_dp(current_pressure - conf.data.last_pressure)
conf.data.last_pressure = current_pressure
# IMU
data["sensors"]["acc"] = sense.get_accelerometer_raw() # Gs
data["sensors"]["pitch"] = sense.get_accelerometer()[
"pitch"] # degrees
data["sensors"]["yaw"] = sense.get_accelerometer()["yaw"] # degrees
data["sensors"]["roll"] = sense.get_accelerometer()["roll"] # degrees
data["sensors"]["compass"] = sense.get_compass() # rad/sec
data["sensors"]["gyro"] = sense.get_gyroscope_raw() # rad/sec
data["sensors"]["mag"] = sense.get_compass_raw() # microteslas
class SenseHAT(Block, EnrichSignals):
imu = ObjectProperty(IMUsensor, title='IMU Sensor')
version = VersionProperty('0.1.0')
def __init__(self):
super().__init__()
self.hat = None
def configure(self, context):
super().configure(context)
self.hat = SenseHat()
self.hat.set_imu_config(
self.imu().accel(),
self.imu().compass(),
self.imu().gyro())
def process_signals(self, signals):
data = {}
if self.imu().accel():
data['accelerometer'] = self.hat.get_accelerometer_raw()
if self.imu().compass():
data['compass'] = self.hat.get_compass_raw()
if self.imu().gyro():
data['gyroscope'] = self.hat.get_gyroscope_raw()
outgoing_signals = []
for signal in signals:
outgoing_signals.append(self.get_output_signal(data, signal))
self.notify_signals(outgoing_signals)
def sense_data():
sense = SenseHat()
comx, comy, comz = sense.get_compass_raw().values()
accx, accy, accz = sense.get_accelerometer_raw().values()
gyrox, gyroy, gyroz = sense.get_accelerometer_raw().values()
temperature = sense.get_temperature_from_humidity()
humidity = sense.get_humidity()
pressure = sense.get_pressure()
timestamp = datetime.now().isoformat()
if accy > 0.1 :
drop_flg = 1
else:
drop_flg = 0
message = { "deviceid": deviceid, \
"timestamp" : timestamp, \
"temperature" : temperature, \
"humidity" : humidity, \
"pressure" : pressure, \
"comx" : comx, \
"comy" : comy, \
"comz" : comz, \
"gyrox" : gyrox, \
"gyroy" : gyroy, \
"gyroz" : gyroz, \
"accx" : accx, \
"accy" : accy, \
"accz" : accz, \
"drop" : drop_flg
}
print accx, accy, accz, drop_flg
return message
class SensePublisher:
def __init__(self):
self.sense = SenseHat()
self.sense.clear()
self.pub_humidity = rospy.Publisher('sensehat/humidity',
Float64,
queue_size=10)
self.pub_temperature = rospy.Publisher('sensehat/temperature',
Float64,
queue_size=10)
self.pub_pressure = rospy.Publisher('sensehat/pressure',
Float64,
queue_size=10)
self.pub_accelerometer = rospy.Publisher('sensehat/accelerometer',
Vector3,
queue_size=10)
self.pub_gyroscope = rospy.Publisher('sensehat/gyroscope',
Vector3,
queue_size=10)
self.pub_magnetometer = rospy.Publisher('sensehat/magnetometer',
Vector3,
queue_size=10)
self.pub_compass = rospy.Publisher('sensehat/compass',
Float64,
queue_size=10)
self.pub_stick = rospy.Publisher('sensehat/stick',
SenseInputEvent,
queue_size=10)
def publish(self):
self.pub_humidity.publish(self.sense.get_humidity())
self.pub_temperature.publish(self.sense.get_temperature())
self.pub_pressure.publish(self.sense.get_pressure())
acceleration = self.sense.get_accelerometer_raw()
self.pub_accelerometer.publish(
Vector3(acceleration['x'], acceleration['y'], acceleration['z']))
gyroscope = self.sense.get_gyroscope_raw()
self.pub_gyroscope.publish(
Vector3(gyroscope['x'], gyroscope['y'], gyroscope['z']))
compass = self.sense.get_compass_raw()
self.pub_magnetometer.publish(
Vector3(compass['x'], compass['y'], compass['z']))
self.pub_compass.publish(self.sense.get_compass())
stickEvents = self.sense.stick.get_events()
if len(stickEvents) > 0:
event = SenseInputEvent(stickEvents[-1].direction,
stickEvents[-1].action)
self.pub_stick.publish(event)
def turn_off(self):
self.sense.clear()
def run(self):
rospy.init_node('sense_hat', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
self.publish()
rate.sleep()
class SenseLogger:
def __init__(self):
self.sense = SenseHat()
self.filename = "./logs/Senselogg-" + str(datetime.now()) + ".csv"
self.file_setup(self.filename)
def write_line(self, line):
with open(self.filename, "a") as f:
f.write(line + "\n")
def log_data(self):
sense_data = self.get_sense_data()
line = ",".join(str(value) for value in sense_data)
self.write_line(line)
def file_setup(self, filename):
header = [
"datetime", "temp_h", "temp_p", "humidity", "pressure", "pitch",
"roll", "yaw", "mag_x", "mag_y", "mag_z", "accel_x", "accel_y",
"accel_z", "gyro_x", "gyro_y", "gyro_z"
]
with open(filename, "w") as f:
f.write(",".join(str(value) for value in header) + "\n")
def get_sense_data(self):
sense_data = []
sense_data.append(datetime.now())
sense_data.append(self.sense.get_temperature_from_humidity())
sense_data.append(self.sense.get_temperature_from_pressure())
sense_data.append(self.sense.get_humidity())
sense_data.append(self.sense.get_pressure())
o = self.sense.get_orientation()
yaw = o["yaw"]
pitch = o["pitch"]
roll = o["roll"]
sense_data.extend([pitch, roll, yaw])
mag = self.sense.get_compass_raw()
x = mag["x"]
y = mag["y"]
z = mag["z"]
sense_data.extend([x, y, z])
acc = self.sense.get_accelerometer_raw()
x = acc["x"]
y = acc["y"]
z = acc["z"]
sense_data.extend([x, y, z])
gyro = self.sense.get_gyroscope_raw()
x = gyro["x"]
y = gyro["y"]
z = gyro["z"]
sense_data.extend([x, y, z])
return sense_data
class SenseLogger:
def __init__(self):
self.sense = SenseHat()
self.filename = "./logs/Senselogg-"+str(datetime.now())+".csv"
self.file_setup(self.filename)
def write_line(self, line):
with open(self.filename, "a") as f:
f.write(line + "\n")
def log_data(self):
sense_data = self.get_sense_data()
line = ",".join(str(value) for value in sense_data)
self.write_line(line)
def file_setup(self, filename):
header = ["datetime", "temp_h", "temp_p", "humidity", "pressure", "pitch",
"roll", "yaw", "mag_x", "mag_y", "mag_z",
"accel_x", "accel_y", "accel_z",
"gyro_x", "gyro_y", "gyro_z"]
with open(filename, "w") as f:
f.write(",".join(str(value) for value in header)+ "\n")
def get_sense_data(self):
sense_data = []
sense_data.append(datetime.now())
sense_data.append(self.sense.get_temperature_from_humidity())
sense_data.append(self.sense.get_temperature_from_pressure())
sense_data.append(self.sense.get_humidity())
sense_data.append(self.sense.get_pressure())
o = self.sense.get_orientation()
yaw = o["yaw"]
pitch = o["pitch"]
roll = o["roll"]
sense_data.extend([pitch, roll, yaw])
mag = self.sense.get_compass_raw()
x = mag["x"]
y = mag["y"]
z = mag["z"]
sense_data.extend([x, y, z])
acc = self.sense.get_accelerometer_raw()
x = acc["x"]
y = acc["y"]
z = acc["z"]
sense_data.extend([x, y, z])
gyro = self.sense.get_gyroscope_raw()
x = gyro["x"]
y = gyro["y"]
z = gyro["z"]
sense_data.extend([x, y, z])
return sense_data
def Main():
export_data("temp", "hum", "pres", "time", "pos")
sense = SenseHat()
sense.clear()
while True:
time.sleep(600)
ts = int(time.time())
temp = sense.get_temperature()
hum = sense.get_humidity()
pres = sense.get_pressure()
pos = sense.get_compass_raw()
export_data(temp, hum, pres, ts, pos)
class SenseController:
def __init__(self):
self.sense = SenseHat()
def __enter__(self):
self.clear()
return self
def __exit__(self, type, value, traceback):
self.clear()
def clear(self):
self.sense.clear()
def get_data(self):
orientation_time = time.time()
orientation = self.sense.get_orientation_degrees()
compass_time = time.time()
compass = self.sense.get_compass_raw()
acceleration_time = time.time()
acceleration = self.sense.get_accelerometer_raw()
return [
# Environmental sensors
(time.time(), "humidity", self.sense.get_humidity()),
(time.time(), "pressure", self.sense.get_pressure()),
(time.time(), "temperature_from_humidity",
self.sense.get_temperature()),
(time.time(), "temperature_from_pressure",
self.sense.get_temperature_from_pressure()),
# IMU sensors
(orientation_time, "orientation.pitch", orientation['pitch']),
(orientation_time, "orientation.roll", orientation['roll']),
(orientation_time, "orientation.yaw", orientation['yaw']),
(compass_time, "compass.x", compass['x']),
(compass_time, "compass.y", compass['y']),
(compass_time, "compass.z", compass['z']),
(acceleration_time, "accelerometer.x", acceleration['x']),
(acceleration_time, "accelerometer.y", acceleration['y']),
(acceleration_time, "accelerometer.z", acceleration['z'])
]
def show_message(self, message):
self.sense.show_message(message, text_colour=[0, 64, 0])
class IMU:
def __init__(self):
self.sense = SenseHat()
self.sense.clear()
self.zero_pressure = self.sense.get_pressure()
def get_accelerometer():
return self.sense.get_accelerometer()
def get_compass():
return self.sense.get_compass()
def get_accerometer_raw():
return self.sense.get_accelerometer_raw()
def get_gyroscope_raw():
return self.sense.get_gyroscope_raw()
def get_compass_raw():
return self.sense.get_compass_raw()
def publishOtherInfo():
sense = SenseHat()
while True:
try:
# get temperature data
temp = sense.get_temperature()
temp = round(temp, 1)
publish('temperature', {'temperature': temp})
print('Temperature: ' + str(temp) + '; ')
# get humidity data
hum = sense.get_humidity()
hum = round(hum, 1)
publish('humidity', {'humidity': hum})
print('Humidity: ' + str(hum) + '; ')
# get pressure data
pre = sense.get_pressure()
pre = round(pre, 1)
publish('pressure', {'pressure': pre})
print('Pressure: ' + str(pre))
# get compass data
compass_north = sense.get_compass()
compass_north = round(compass_north, 1)
compass_data = sense.get_compass_raw()
m_x = compass_data['x']
m_x = round(m_x, 1)
m_y = compass_data['y']
m_y = round(m_y, 1)
m_z = compass_data['z']
m_z = round(m_z, 1)
publish('compass', {'north':compass_north,'m_x': m_x, 'm_y': m_y, 'm_z': m_z})
print('North: ' + str(compass_north) + ' Compass_x: ' + str(m_x) + ' Compass_y: ' + str(m_y) + ' Compass_z: ' + str(m_z) + '\n')
time.sleep(3)
except IOError:
print("IOError")
class SenseClient(object):
def __init__(self):
self.sense = SenseHat()
self.sense.clear()
self.sense.set_imu_config(True, True, True)
def getSensePoints(self, imperial_or_metric, bucket):
dt = datetime.now(tz=pytz.timezone('US/Pacific')).isoformat()
point = Point(measurement_name="sense")
point.time(time=dt)
# % relative
point.field("humidity", self.sense.get_humidity())
if imperial_or_metric == "imperial":
point.field(
"temperature_from_humidity",
convertCToF(self.sense.get_temperature_from_humidity()))
point.field(
"temperature_from_pressure",
convertCToF(self.sense.get_temperature_from_pressure()))
point.field("pressure", convertmbToPSI(self.sense.get_pressure()))
else:
point.field("temperature_from_humidity",
self.sense.get_temperature_from_humidity())
point.field("temperature_from_pressure",
self.sense.get_temperature_from_pressure())
point.field("pressure", self.sense.get_pressure())
point.field("orientation_radians",
self.sense.get_orientation_radians())
point.field("orientation_degress",
self.sense.get_orientation_degrees())
# magnetic intensity in microteslas
point.field("compass_raw", self.sense.get_compass_raw())
# rotational intensity in radians per second
point.field("gyroscope_raw", self.sense.get_gyroscope_raw())
# acceleration intensity in Gs
point.field("accelerometer_raw", self.sense.get_accelerometer_raw())
return [{"bucket": bucket, "point": point}]
class SenseHAT(Block, EnrichSignals):
env = ObjectProperty(EnvironmentalSensors,
title='Environmental Sensors',
order=0)
imu = ObjectProperty(IMUsensor, title='IMU Sensor', order=1)
version = VersionProperty('0.1.0')
def __init__(self):
super().__init__()
self.hat = None
def configure(self, context):
super().configure(context)
self.hat = SenseHat()
self.hat.set_imu_config(self.imu().accel(),
self.imu().compass(),
self.imu().gyro())
def process_signals(self, signals):
data = {}
if self.imu().accel():
data['accelerometer'] = self.hat.get_accelerometer_raw()
if self.imu().compass():
data['compass'] = self.hat.get_compass_raw()
if self.imu().gyro():
data['gyroscope'] = self.hat.get_gyroscope_raw()
if self.env().rh():
data['relative_humidity'] = self.hat.get_humidity()
if self.env().temp():
data['temperature_C'] = self.hat.get_temperature()
if self.env().press():
data['pressure_mbar'] = self.hat.get_pressure()
outgoing_signals = []
for signal in signals:
outgoing_signals.append(self.get_output_signal(data, signal))
self.notify_signals(outgoing_signals)
def get_sense_data():
sense = SenseHat()
# Define list
sense_data = []
# Append sensor data
sense_data.append(sense.get_temperature_from_humidity())
sense_data.append(sense.get_temperature_from_pressure())
sense_data.append(sense.get_pressure())
sense_data.append(sense.get_humidity())
# Get orientation from sensor
yaw,pitch,roll = sense.get_orientation().values()
sense_data.extend([pitch,roll,yaw])
# Get magnetic field (compass)
mag_x,mag_y,mag_z = sense.get_compass_raw().values()
sense_data.extend([mag_x,mag_y,mag_z])
# Get accelerometer values
x,y,z = sense.get_accelerometer_raw().values()
sense_data.extend([x,y,z])
# Get Gyro values
gyro_x,gyro_y,gyro_z = sense.get_gyroscope_raw().values()
sense_data.extend([gyro_x,gyro_y,gyro_z])
# Add capture time
sense_data.append(datetime.now())
# Add precise time
sense_data.append(int(round(time.time()*1000)))
return sense_data
from sense_hat import SenseHat import time hat = SenseHat() fill = (255, 0, 0) while True: reading = int(hat.get_compass_raw()['z'] if reading > 200: hat.clear(fill) time.sleep(0.2) else: hat.clear()
from sense_hat import SenseHat
from time import *
sense = SenseHat()
bval = bx = by = bz = 0
b = 0
while 1 > 0:
bval = sense.get_compass_raw()
(bx, by, bz) = bval
b = (bx * bx + by * by + bz * bz)**0.5
print(b)
sleep(0.2)
async def handle(self, context: RequestContext, responder: BaseResponder):
"""
Message handler logic for basic messages.
Args:
context: request context
responder: responder callback
"""
self._logger.debug(f"ReadSensorHandler called with context {context}")
assert isinstance(context.message, ReadSensor)
self._logger.info("Received read sensor: %s", context.message.sensors)
sensors = context.message.sensors
meta = {"sensors": sensors}
conn_mgr = ConnectionManager(context)
await conn_mgr.log_activity(
context.connection_record,
"read_sensor",
context.connection_record.DIRECTION_RECEIVED,
meta,
)
await responder.send_webhook(
"read_sensor",
{
"message_id": context.message._id,
"sensors": sensors,
"state": "received"
},
)
sense = SenseHat()
temperature = None
humidity = None
pressure = None
orientation = None
accelerometer = None
compass = None
gyroscope = None
stick_events = None
pixels = None
if "temperature" in sensors:
temperature = sense.get_temperature()
if "humidity" in sensors:
humidity = sense.get_humidity()
if "pressure" in sensors:
pressure = sense.get_pressure()
if "orientation" in sensors:
orientation = sense.get_orientation_degrees()
if "accelerometer" in sensors:
accelerometer = sense.get_accelerometer_raw()
if "compass" in sensors:
compass = sense.get_compass_raw()
if "gyroscope" in sensors:
gyroscope = sense.get_gyroscope_raw()
if "stick_events" in sensors:
stick_events = []
stick_event_objects = sense.stick.get_events()
for event in stick_event_objects:
event_dict = {}
event_dict['timestamp'] = event.timestamp
event_dict['direction'] = event.direction
event_dict['action'] = event.action
stick_events.append(event_dict)
if "pixels" in sensors:
pixels = sense.get_pixels()
reply_msg = SensorValue(temperature=temperature,
humidity=humidity,
pressure=pressure,
orientation=orientation,
accelerometer=accelerometer,
compass=compass,
gyroscope=gyroscope,
stick_events=stick_events,
pixels=pixels)
await responder.send_reply(reply_msg)
await conn_mgr.log_activity(
context.connection_record,
"sensor_value",
context.connection_record.DIRECTION_SENT,
{"content": "reply"},
)
class CiosRaspberryHat:
#
# Constructor
#
# url string WebsocketのURL
# channel_id string CiosのチャネルID
# access_token string Ciosのアクセストークン
#
def __init__(self, url, channel_id, access_token):
self.url = url
self.channel = channel_id
self.token = access_token
self.ws = 0
self.sense = SenseHat() # SenseHat インスタンス作成
self.sense.set_imu_config(True, True, True) # 加速度センサーの有効化
self.connectCount = 0
self.screen = "top"
#
# connection
#
# WebSocketへのコネクションを貼る
#
def connection(self):
try:
ws_url = self.url + "?" + "channel_id=" + self.channel + "&access_token=" + self.token
print("--------------- WebSocket Connection ---------------")
print("ConnectionURL: " + ws_url)
print("--------------- WebSocket Connection ---------------")
self.sense.show_letter("C", text_colour=[0, 255, 255])
self.ws = create_connection(ws_url)
except:
print("Websocket Connection Error...")
self.sense.show_letter("E", text_colour=[255, 0, 0])
self.errorReConnect()
return "error"
#
# getSensorData
#
# SenseHatからのデータを取得、JSON整形を行う
# return: Json String
#
def getSensorData(self):
try:
humidity = self.sense.get_humidity()
temp = self.sense.get_temperature()
pressure = self.sense.get_pressure()
orientation = self.sense.get_orientation_degrees()
compass = self.sense.get_compass_raw()
gyroscope = self.sense.get_gyroscope_raw()
accelerometer = self.sense.get_accelerometer_raw()
message = {
"humidity": humidity,
"temperature": temp,
"pressure": pressure,
"degrees_p": orientation["pitch"],
"degrees_r": orientation["roll"],
"degrees_y": orientation["yaw"],
"compass_x": compass["x"],
"compass_y": compass["y"],
"compass_z": compass["z"],
"gyroscope_x": gyroscope["x"],
"gyroscope_y": gyroscope["y"],
"gyroscope_z": gyroscope["z"],
"accelerometer_x": accelerometer["x"],
"accelerometer_y": accelerometer["y"],
"accelerometer_z": accelerometer["z"]
}
send_message = json.dumps(message)
return send_message
except:
print("getSensorData Error...")
self.ws.close()
self.sense.show_letter("E", text_colour=[255, 0, 0])
self.errorReConnect()
return "error"
#
# sendMessage
#
# message string 送信するメッセージ（Json形式）
#
def sendMessage(self, message):
try:
print("--------------- WebSocket Send Message ---------------")
print(message)
print("--------------- WebSocket Send Message ---------------")
self.ws.send(message)
except:
print("Websocket Send Error...")
self.ws.close()
self.sense.show_letter("E", text_colour=[255, 0, 0])
self.errorReConnect()
return "error"
#
# Error ReConnect WebSocket
#
# message string 送信するメッセージ（Json形式）
#
def errorReConnect(self):
try:
if self.connectCount > 0: # ErrorCount カウント数以上のエラーで停止
raise
self.connectCount += 1
for v in range(self.connectCount): # Error数に応じて、待機時間を追加
print("Wait ::: " + str(self.connectCount - v) + " sec")
time.sleep(1)
self.connection()
except:
print("Websocket connection Error count : " +
str(self.connectCount) + " Over")
self.sense.show_letter("E", text_colour=[255, 0, 0])
time.sleep(0.5)
self.sense.show_letter("E", text_colour=[0, 255, 0])
time.sleep(0.5)
self.sense.show_letter("E", text_colour=[0, 0, 255])
time.sleep(0.5)
self.sense.show_letter("E", text_colour=[255, 0, 0])
time.sleep(2)
self.sense.clear()
exit()
from sense_hat import SenseHat
sense = SenseHat()
raw = sense.get_compass_raw()
print("x: {x}, y: {y}, z: {z}".format(**raw))
# alternatives
print(sense.compass_raw)
m_y_old=0
m_z_old=0
oldYaw=0
oldNorma=0
oldNormaW=0
while True:
temperature = sense.get_temperature()
pressure = sense.get_pressure()
humidity = sense.get_humidity()
pitch, roll, yaw = sense.get_orientation_degrees().values()
x, y, z = sense.get_accelerometer_raw().values()
m_x, m_y, m_z = sense.get_compass_raw().values()
# Rotation
pitch = round(pitch, 3)
roll = round(roll, 3)
yaw = round(yaw, 3)
# Enviromental
temperature = round(temperature, 1)
pressure = round(pressure, 1)
humidity = round(humidity, 1)
# Accelerometer
# Get data
sense = SenseHat()
previousTime = -1
ACCELEROMETER_DRIFT_WHEN_STATIONARY = 0.00000000001
samplePeriod = 1 / 256
posX = 0
posY = 0
posZ = 0
while True:
# Get sensor data and the current time
gyro = sense.get_gyroscope_raw() # deg/s
acc = sense.get_accelerometer_raw() # g's (1g = 9.81 m/s^2)
mag = sense.get_compass_raw() # microteslas (uT)
currentTime = time()
# Calculate the magnitude of acceleration
accMagnitude = sqrt((acc['x'] * acc['x']) + (acc['y'] * acc['y']) +
(acc['z'] * acc['z']))
# Use a high-pass filter to remove some noise
filterCutoff = 0.001
butterFilterB, butterFilterA = signal.butter(
1, (2 * filterCutoff) / (1 / samplePeriod), 'highpass')
accMagnitudeFiltered = signal.filtfilt(butterFilterB,
butterFilterA,
[accMagnitude, accMagnitude],
padlen=1)
from sense_hat import SenseHat
import time
hat = SenseHat()
fill = (255, 0, 0)
while True:
reading = int(hat.get_compass_raw()['z'])
if reading > 200:
hat.clear(fill)
time.sleep(0.2)
else:
hat.clear()
#!/usr/bin/python3 -u
import time
import quaternion
from madgwick import MadgwickAHRS
from sense_hat import SenseHat
heading = MadgwickAHRS()
sense = SenseHat()
while True:
gyro = [value for key, value in sense.get_gyroscope_raw().items()]
accel = [value for key, value in sense.get_accelerometer_raw().items()]
compass = [value for key, value in sense.get_compass_raw().items()]
heading.update(gyro, accel, compass)
ahrs = heading.quaternion.to_euler_angles()
roll = ahrs[0]
pitch = ahrs[1]
yaw = ahrs[2]
#(p,r,y) = heading.quaternion.to_euler_angles()
print(pitch, roll, yaw)
time.sleep(0.1)
iss.compute()
lat = str(iss.sublat).split(':')
long = str(iss.sublong).split(':')
#get pitch, roll and yaw orientation from gyro
o = sense.get_orientation()
gyro_pitch = o['pitch']
gyro_roll = o['roll']
gyro_yaw = o['yaw']
#get acceleration
acc_x, acc_y, acc_z = sense.get_accelerometer_raw().values()
acc_roll, acc_pitch, acc_yaw = sense.get_accelerometer().values()
#get magnetometer values
mag_x, mag_y, mag_z = sense.get_compass_raw().values()
compass = sense.get_compass()
#get gyro velocity
gyro_vel = sense.get_gyroscope_raw()
gyro_vel_x = gyro_vel['x']
gyro_vel_y = gyro_vel['y']
gyro_vel_z = gyro_vel['z']
#get humidity
humidity = sense.get_humidity()
#get temperature
temperature = sense.get_temperature()
#get pressure
class DataWrite:
def __init__(self):
self.sense = SenseHat()
self.sense.set_imu_config(True, True, True)
def get_data(self):
"""Gets data from environmental sensors and IMU sensors
and formats it for writing to a CSV with time as the first item
"""
# get environmental data from the sensehat
def get_enviro():
"""Gets environmental data and formats it in the form:
pressure, temperature_pressure, temperature_humidity, humidity
"""
# Get readings from each sensor
pressure = self.sense.get_pressure()
temp_press = self.sense.get_temperature_from_pressure()
temp_humid = self.sense.get_temperature_from_humidity()
humidity = self.sense.get_humidity()
# Format the readings
enviro_results = [pressure, temp_press, temp_humid, humidity]
return enviro_results
# get IMU data from the sensehat
def get_imu():
"""Gets IMU data and formats it in the form:
accelX, accelY, accelZ, gyroX, gyroY, gyroZ, compassX, compassY, compassZ, orientationX, orientationY,
orientationZ
"""
# get raw data from IMU sensors
accelraw = self.sense.get_accelerometer_raw()
gyroraw = self.sense.get_gyroscope_raw()
compassraw = self.sense.get_compass_raw()
orientationraw = self.sense.get_orientation_degrees()
# Format raw data into a usable list
imu_results = [
accelraw['x'], accelraw['y'], accelraw['z'], gyroraw['x'],
gyroraw['y'], gyroraw['z'], compassraw['x'], compassraw['y'],
compassraw['z'], orientationraw['pitch'],
orientationraw['roll'], orientationraw['yaw']
]
return imu_results
# Get data from sensors and add time then append together
enviro_res = get_enviro()
imu_res = get_imu()
current_time = datetime.datetime.now().strftime(
"%d-%b-%Y (%H:%M:%S.%f)")
results = [current_time]
results.extend(enviro_res)
results.extend(imu_res)
print(results)
return results
def write_data(self):
"""Writes data to data.csv in append mode as to not delete headers or previous data"""
with open('data.csv', 'a') as f:
writer = csv.writer(f)
writer.writerow(self.get_data())
SH_pressure = sense.get_pressure() * 100 # convert output from millibars to Pascals for consistency
SH_humidity = sense.get_humidity() # % relative humidity
# Orientation
sense.set_imu_config(True,True,True) # Enable compass, gyro, and accelerometer
SH_orientation = sense.get_orientation() # orientation of pitch, roll, yaw axes in degrees
SH_orientation_x = SH_orientation.get('x')
SH_orientation_y = SH_orientation.get('y')
SH_orientation_z = SH_orientation.get('z')
# Magnetometer data
#sense.set_imu_config(True,False,False)
time.sleep(0.01) # sleep for 10 ms after changing IMU configuration
SH_compass_north = sense.get_compass() # direction of magnetometer from North, in degrees
SH_compass_raw = sense.get_compass_raw() # magnetic intensity of x, y, z axes in microteslas
SH_compass_raw_x = SH_compass_raw.get('x')
SH_compass_raw_y = SH_compass_raw.get('y')
SH_compass_raw_z = SH_compass_raw.get('z')
# Gyro Data
#sense.set_imu_config(False,True,False)
time.sleep(0.01) # sleep for 10 ms after changing IMU configuration
#SH_gyro = sense.get_gyroscope() # orientation of pitch, roll, yaw axes in degrees
SH_gyro_raw = sense.get_gyroscope_raw() # rotational velocity of pitch, roll, yaw axes in radians per sec
SH_gyro_raw_x = SH_gyro_raw.get('x')
SH_gyro_raw_y = SH_gyro_raw.get('y')
SH_gyro_raw_z = SH_gyro_raw.get('z')
# Accelerometer data
#sense.set_imu_config(False,False,True)
start = time.time()
current = time.time()
i = 0
video = 0
#SET MAX LOG DURATION IN SECONDS
while (current-start) < 5:
current = time.time()
t = sense.get_temperature()
p = sense.get_pressure()
h = sense.get_humidity()
pitch, roll, yaw = sense.get_orientation().values()
xc, yc, zc = sense.get_compass_raw().values()
xg, yg, zg = sense.get_gyroscope_raw().values()
xa, ya, za = sense.get_accelerometer_raw().values()
f = open('./hat-log/hat.csv', 'a', os.O_NONBLOCK)
line = "%d, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n" % (1000*time.time(),t,p,h,pitch,roll,yaw,xc,yc,zc,xg,yg,zg,xa,ya,za)
f.write(line)
f.flush
f.close()
#set za threshold to the number of g you would expect at launch
if video == 0 and (za > 1.1 or za < -1.1):
video = 1
call(["./video", ""])
#print i
class InputModule(AbstractInput):
""" A sensor support class that measures """
def __init__(self, input_dev, testing=False):
super(InputModule, self).__init__(input_dev,
testing=testing,
name=__name__)
self.sensor = None
if not testing:
self.initialize_input()
def initialize_input(self):
""" Initialize the Sense HAT sensor class """
from sense_hat import SenseHat
self.sensor = SenseHat()
def get_measurement(self):
""" Get measurements and store in the database """
if not self.sensor:
self.logger.error("Input not set up")
return
self.return_dict = copy.deepcopy(measurements_dict)
if self.is_enabled(0):
try:
self.value_set(0, self.sensor.get_temperature())
except Exception as e:
self.logger.error(
"Temperature (temperature sensor) read failure: {}".format(
e))
if self.is_enabled(1):
try:
self.value_set(1, self.sensor.get_temperature_from_humidity())
except Exception as e:
self.logger.error(
"Temperature (humidity sensor) read failure: {}".format(e))
if self.is_enabled(2):
try:
self.value_set(2, self.sensor.get_temperature_from_pressure())
except Exception as e:
self.logger.error(
"Temperature (pressure sensor) read failure: {}".format(e))
if self.is_enabled(3):
try:
self.value_set(3, self.sensor.get_humidity())
except Exception as e:
self.logger.error("Humidity read failure: {}".format(e))
if self.is_enabled(4):
try:
self.value_set(4, self.sensor.get_pressure())
except Exception as e:
self.logger.error("Pressure read failure: {}".format(e))
if self.is_enabled(5):
try:
self.value_set(5, self.sensor.get_compass())
except Exception as e:
self.logger.error("Compass read failure: {}".format(e))
if self.is_enabled(6) or self.is_enabled(7) or self.is_enabled(8):
magnetism = self.sensor.get_compass_raw()
if self.is_enabled(6):
try:
self.value_set(6, magnetism["x"])
except Exception as e:
self.logger.error(
"Compass raw x read failure: {}".format(e))
if self.is_enabled(7):
try:
self.value_set(7, magnetism["y"])
except Exception as e:
self.logger.error(
"Compass raw y read failure: {}".format(e))
if self.is_enabled(8):
try:
self.value_set(8, magnetism["z"])
except Exception as e:
self.logger.error(
"Compass raw z read failure: {}".format(e))
if self.is_enabled(9) or self.is_enabled(10) or self.is_enabled(11):
gyroscope = self.sensor.get_gyroscope()
if self.is_enabled(9):
try:
self.value_set(9, gyroscope["pitch"])
except Exception as e:
self.logger.error(
"Gyroscope pitch read failure: {}".format(e))
if self.is_enabled(10):
try:
self.value_set(10, gyroscope["roll"])
except Exception as e:
self.logger.error(
"Gyroscope roll read failure: {}".format(e))
if self.is_enabled(11):
try:
self.value_set(11, gyroscope["yaw"])
except Exception as e:
self.logger.error(
"Gyroscope yaw read failure: {}".format(e))
if self.is_enabled(12) or self.is_enabled(13) or self.is_enabled(14):
acceleration = self.sensor.get_accelerometer_raw()
if self.is_enabled(12):
try:
self.value_set(12, acceleration["x"])
except Exception as e:
self.logger.error(
"Acceleration x read failure: {}".format(e))
if self.is_enabled(13):
try:
self.value_set(13, acceleration["y"])
except Exception as e:
self.logger.error(
"Acceleration y read failure: {}".format(e))
if self.is_enabled(14):
try:
self.value_set(14, acceleration["z"])
except Exception as e:
self.logger.error(
"Acceleration z read failure: {}".format(e))
return self.return_dict
import time
import datetime
from sense_hat import SenseHat
sense = SenseHat()
red = (255, 0, 0)
blue = (0, 0, 255)
while True:
temp = sense.get_temperature()
humidity = sense.get_humidity()
pressure = sense.get_pressure()
gyro = sense.get_gyroscope()
rawMag = sense.get_compass_raw()
timestamp = time.ctime()
accel_only = sense.get_accelerometer()
north = sense.get_compass()
print("____________________________________________________________________")
print("Temperature: %s C" % temp)
print("Humidity: %s %%rH" % humidity)
print("Pressure: %s Millibars" % pressure)
print("North: %s" % north)
print("Magnetometer x: {x}, y: {y}, z: {z}".format(**rawMag))
print("Gyro: p: {pitch}, r: {roll}, y: {yaw}".format(**gyro))
print("Accel: p: {pitch}, r: {roll}, y: {yaw}".format(**accel_only))
log_record = "%s : temp=%s humidity=%s pressure=%s gyro=%s accel=%s mag=%s" % (
timestamp,
from sense_hat import SenseHat
import time
sh = SenseHat()
while True:
# 磁気センサの生データ。Dicitonary型
raw = sh.get_compass_raw()
x_val = round(float(raw['x']),2)
y_val = round(float(raw['y']),2)
z_val = round(float(raw['z']),2)
# 北からの位置を取得。加速度センサと磁気センサの値から計算している。範囲は0〜360、単位は度。0が北、180が南。
# 値はゆっくり変化していく、方位磁石のほうが精度が良い
north = sh.get_compass()
print("North: %s (x=%s, y=%s, z=%s)" %(north, x_val, y_val, z_val))
time.sleep(1)
# Python SenseHat API documentation:
# https://pythonhosted.org/sense-hat/api
# Roll is: angular x.
# Pitch is: angular y.
# Yaw is: angular z.
data_imu = {}
data_imu["gyro"] = sense.get_orientation_degrees(
) # Gets orientation (3-ang-axis) in [deg].
data_imu["acc"] = sense.get_accelerometer(
) # Gets orientation from the accelerometer (3-ang-axis) in [deg].
data_imu["gyror"] = sense.get_gyroscope_raw(
) # Gets angular velocity (3-axis) in [rad/s].
data_imu["mag"] = sense.get_compass() # Gets orientation to North in [deg].
data_imu["magr"] = sense.get_compass_raw(
) # Gets magnetic field (3-axis) in [µT].
data_imu["accr"] = sense.get_accelerometer_raw(
) # Gets acceleration (3-axis) in [g]'s.
data_env = {}
data_env["temph"] = sense.get_temperature_from_humidity(
) # Gets temperature from humidity sensor in [ºC].
data_env["tempp"] = sense.get_temperature_from_pressure(
) # Gets temperature from pressure sensor in [ºC].
data_env["pres"] = sense.get_pressure() # Gets pressure in [mbar].
data_env["hum"] = sense.get_humidity() # Gets relative humidity in [%].
cpu_temp = os.popen("vcgencmd measure_temp").readline()
data_env["tempcpu"] = float(cpu_temp.replace("temp=", "").replace("'C\n", ""))
data = {}
data["env"] = data_env
acceleration = sense.get_accelerometer_raw()
AcX = acceleration["x"]
AcY = acceleration["y"]
AcZ = acceleration["z"]
gyroscope = sense.get_gyroscope_raw()
GyX = gyroscope["x"]
GyY = gyroscope["y"]
GyZ = gyroscope["z"]
if start == 54:
start = 0
magnetometer = sense.get_compass_raw()
MaX = magnetometer["x"]
MaY = magnetometer["y"]
MaZ = magnetometer["z"]
magnetometer_equation = sqrt(MaX * MaX + MaY * MaY + MaZ * MaZ)
with open("date.csv", "a") as file:
file.write(date)
file.write(", ")
file.write(str(real_temp))
file.write(", ")
file.write(str(real_humidity))
file.write(", ")
file.write(str(pressure))
'''Test function:
lat = '4142.86751'
longi = '08803.24426'
loca_format(lat) + "," + loca_format(longi)
'''
# Publish to the same topic in a loop forever
while True:
gpsList = [] # save ALL sensor and gps data (10 rows)
if args.mode == 'both' or args.mode == 'publish':
pitch, roll, yaw = sense.get_orientation().values()
ax, ay, az = sense.get_accelerometer_raw().values()
mx, my, mz = sense.get_compass_raw().values()
message = {}
message['camera_id'] = 'raspberry1'
# sensor
message['gx'] = pitch
message['gy'] = roll
message['gz'] = yaw
message['ax'] = ax
message['ay'] = ay
message['az'] = az
key=bytes("alert", encoding='utf-8'),
value=bytes(message, encoding='utf-8'))
producer.send(device,
key=bytes("alert", encoding='utf-8'),
value=bytes(message, encoding='utf-8'))
elif event.action == oldAction and event.action == 'held' and oldDirection != event.direction:
oldDirection = event.direction
producer.send('alerts',
key=bytes("alert", encoding='utf-8'),
value=bytes(message, encoding='utf-8'))
oldAction = event.action
sense.stick.direction_any = mid
while True:
readings = {}
readings['orentation'] = sense.get_orientation()
readings['compass'] = sense.get_compass_raw()
readings['gyroscope'] = sense.gyro_raw
readings['accelerometer'] = sense.accel_raw
x = x + 1
message = json.dumps(readings)
producer.send(device,
key=bytes("message", encoding='utf-8'),
value=bytes(message, encoding='utf-8'))
time.sleep(parser.getfloat('device', 'sendSleep'))
def start(self):
self.enable = True
self.imuPub = rospy.Publisher(self.robot_host + '/imu',
Imu,
queue_size=10)
self.imuRawPub = rospy.Publisher(self.robot_host + '/imu/raw',
Imu,
queue_size=10)
self.accelerometerPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer',
Accelerometer,
queue_size=10)
self.accelerometerPitchPub = rospy.Publisher(
self.robot_host + '/imu/accelerometer/pitch', Pitch, queue_size=10)
self.accelerometerRollPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/roll',
Roll,
queue_size=10)
self.accelerometerYawPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/yaw',
Yaw,
queue_size=10)
self.accelerometerRawPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/raw',
Accelerometer,
queue_size=10)
self.accelerometerRawXPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/raw/x',
Float64,
queue_size=10)
self.accelerometerRawYPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/raw/y',
Float64,
queue_size=10)
self.accelerometerRawZPub = rospy.Publisher(self.robot_host +
'/imu/accelerometer/raw/z',
Float64,
queue_size=10)
self.gyroscopePub = rospy.Publisher(self.robot_host + '/imu/gyroscope',
Gyroscope,
queue_size=10)
self.gyroscopePitchPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/pitch',
Pitch,
queue_size=10)
self.gyroscopeRollPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/roll',
Roll,
queue_size=10)
self.gyroscopeYawPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/yaw',
Yaw,
queue_size=10)
self.gyroscopeRawPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/raw',
Gyroscope,
queue_size=10)
self.gyroscopeRawXPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/raw/x',
Float64,
queue_size=10)
self.gyroscopeRawYPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/raw/y',
Float64,
queue_size=10)
self.gyroscopeRawZPub = rospy.Publisher(self.robot_host +
'/imu/gyroscope/raw/z',
Float64,
queue_size=10)
self.magnetometerPub = rospy.Publisher(self.robot_host +
'/imu/magnetometer',
Magnetometer,
queue_size=10)
self.magnetometerRawPub = rospy.Publisher(self.robot_host +
'/imu/magnetometer/raw',
Orientation,
queue_size=10)
self.orientationPub = rospy.Publisher(self.robot_host +
'/imu/orientation',
Orientation,
queue_size=10)
self.orientationDegreePub = rospy.Publisher(self.robot_host +
'/imu/orientation/degrees',
Orientation,
queue_size=10)
self.orientationRadiansPub = rospy.Publisher(
self.robot_host + '/imu/orientation/radians',
Orientation,
queue_size=10)
self.orientationNorthPub = rospy.Publisher(self.robot_host +
'/imu/orientation/north',
Magnetometer,
queue_size=10)
sense = SenseHat()
while not rospy.is_shutdown():
accel_only = sense.get_accelerometer()
accel_raw = sense.get_accelerometer_raw()
gyro_only = sense.get_gyroscope()
gyro_raw = sense.get_gyroscope_raw()
north = sense.get_compass()
compass = sense.get_compass_raw()
orientation = sense.get_orientation()
orientation_deg = sense.get_orientation_degrees()
orientation_rad = sense.get_orientation_radians()
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = "/base_link"
imu_msg.linear_acceleration.x = accel_only['pitch']
imu_msg.linear_acceleration.y = accel_only['roll']
imu_msg.linear_acceleration.z = accel_only['yaw']
imu_msg.angular_velocity.x = gyro_only['pitch']
imu_msg.angular_velocity.y = gyro_only['roll']
imu_msg.angular_velocity.z = gyro_only['yaw']
imu_msg.orientation.x = orientation['pitch']
imu_msg.orientation.y = orientation['roll']
imu_msg.orientation.z = orientation['yaw']
imu_msg.orientation.w = 0
imu_msg.orientation_covariance = [
99999.9, 0.0, 0.0, 0.0, 99999.9, 0.0, 0.0, 0.0, 99999.9
]
imu_msg.angular_velocity_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
imu_msg.linear_acceleration_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
imu_raw_msg = Imu()
imu_raw_msg.header.stamp = rospy.Time.now()
imu_raw_msg.header.frame_id = "/base_link"
imu_raw_msg.linear_acceleration.x = accel_raw['x']
imu_raw_msg.linear_acceleration.y = accel_raw['y']
imu_raw_msg.linear_acceleration.z = accel_raw['z']
imu_raw_msg.angular_velocity.x = gyro_raw['x']
imu_raw_msg.angular_velocity.y = gyro_raw['y']
imu_raw_msg.angular_velocity.z = gyro_raw['z']
imu_raw_msg.orientation.x = compass['x']
imu_raw_msg.orientation.y = compass['y']
imu_raw_msg.orientation.z = compass['z']
imu_raw_msg.orientation.w = north
imu_raw_msg.orientation_covariance = [
99999.9, 0.0, 0.0, 0.0, 99999.9, 0.0, 0.0, 0.0, 99999.9
]
imu_raw_msg.angular_velocity_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
imu_raw_msg.linear_acceleration_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
accel_msg = Accelerometer()
accel_msg.header.stamp = rospy.Time.now()
accel_msg.header.frame_id = "/base_link"
accel_msg.x = accel_only['pitch']
accel_msg.y = accel_only['roll']
accel_msg.z = accel_only['yaw']
accel_pitch_msg = Pitch()
accel_pitch_msg.header.stamp = rospy.Time.now()
accel_pitch_msg.header.frame_id = "/base_link"
accel_pitch_msg.data = accel_only['pitch']
accel_roll_msg = Roll()
accel_roll_msg.header.stamp = rospy.Time.now()
accel_roll_msg.header.frame_id = "/base_link"
accel_roll_msg.data = accel_only['roll']
accel_yaw_msg = Yaw()
accel_yaw_msg.header.stamp = rospy.Time.now()
accel_yaw_msg.header.frame_id = "/base_link"
accel_yaw_msg.data = accel_only['yaw']
accel_raw_msg = Accelerometer()
accel_raw_msg.header.stamp = rospy.Time.now()
accel_raw_msg.header.frame_id = "/base_link"
accel_raw_msg.x = accel_raw['x']
accel_raw_msg.y = accel_raw['y']
accel_raw_msg.z = accel_raw['z']
accel_raw_x_msg = Float64()
accel_raw_x_msg.header.stamp = rospy.Time.now()
accel_raw_x_msg.header.frame_id = "/base_link"
accel_raw_x_msg.data = accel_raw['x']
accel_raw_y_msg = Float64()
accel_raw_y_msg.header.stamp = rospy.Time.now()
accel_raw_y_msg.header.frame_id = "/base_link"
accel_raw_y_msg.data = accel_raw['y']
accel_raw_z_msg = Float64()
accel_raw_z_msg.header.stamp = rospy.Time.now()
accel_raw_z_msg.header.frame_id = "/base_link"
accel_raw_z_msg.data = accel_raw['z']
gyro_msg = Gyroscope()
gyro_msg.header.stamp = rospy.Time.now()
gyro_msg.header.frame_id = "/base_link"
gyro_msg.x = gyro_only['pitch']
gyro_msg.y = gyro_only['roll']
gyro_msg.z = gyro_only['yaw']
gyro_pitch_msg = Pitch()
gyro_pitch_msg.header.stamp = rospy.Time.now()
gyro_pitch_msg.header.frame_id = "/base_link"
gyro_pitch_msg.data = gyro_only['pitch']
gyro_roll_msg = Roll()
gyro_roll_msg.header.stamp = rospy.Time.now()
gyro_roll_msg.header.frame_id = "/base_link"
gyro_roll_msg.data = gyro_only['roll']
gyro_yaw_msg = Yaw()
gyro_yaw_msg.header.stamp = rospy.Time.now()
gyro_yaw_msg.header.frame_id = "/base_link"
gyro_yaw_msg.data = gyro_only['yaw']
gyro_raw_msg = Gyroscope()
gyro_raw_msg.header.stamp = rospy.Time.now()
gyro_raw_msg.header.frame_id = "/base_link"
gyro_raw_msg.x = gyro_raw['x']
gyro_raw_msg.y = gyro_raw['y']
gyro_raw_msg.z = gyro_raw['z']
gyro_raw_x_msg = Float64()
gyro_raw_x_msg.header.stamp = rospy.Time.now()
gyro_raw_x_msg.header.frame_id = "/base_link"
gyro_raw_x_msg.data = gyro_raw['x']
gyro_raw_y_msg = Float64()
gyro_raw_y_msg.header.stamp = rospy.Time.now()
gyro_raw_y_msg.header.frame_id = "/base_link"
gyro_raw_y_msg.data = gyro_raw['y']
gyro_raw_z_msg = Float64()
gyro_raw_z_msg.header.stamp = rospy.Time.now()
gyro_raw_z_msg.header.frame_id = "/base_link"
gyro_raw_z_msg.data = gyro_raw['z']
north_msg = Magnetometer()
north_msg.header.stamp = rospy.Time.now()
north_msg.header.frame_id = "/base_link"
north_msg.north = north
compass_msg = Orientation()
compass_msg.header.stamp = rospy.Time.now()
compass_msg.header.stamp = rospy.Time.now()
compass_msg.x = compass['x']
compass_msg.y = compass['y']
compass_msg.z = compass['z']
orientation_msg = Orientation()
orientation_msg.header.stamp = rospy.Time.now()
orientation_msg.header.stamp = rospy.Time.now()
orientation_msg.x = orientation['pitch']
orientation_msg.y = orientation['roll']
orientation_msg.z = orientation['yaw']
orientation_degree_msg = Orientation()
orientation_degree_msg.header.stamp = rospy.Time.now()
orientation_degree_msg.header.stamp = rospy.Time.now()
orientation_degree_msg.x = orientation_deg['pitch']
orientation_degree_msg.y = orientation_deg['roll']
orientation_degree_msg.z = orientation_deg['yaw']
orientation_rad_msg = Orientation()
orientation_rad_msg.header.stamp = rospy.Time.now()
orientation_rad_msg.header.stamp = rospy.Time.now()
orientation_rad_msg.x = orientation_rad['pitch']
orientation_rad_msg.y = orientation_rad['roll']
orientation_rad_msg.z = orientation_rad['yaw']
rospy.loginfo(
"imu/accelerometer: p: {pitch}, r: {roll}, y: {yaw}".format(
**accel_only))
rospy.loginfo(
"imu/accelerometer/raw: x: {x}, y: {y}, z: {z}".format(
**accel_raw))
rospy.loginfo(
"imu/gyroscope: p: {pitch}, r: {roll}, y: {yaw}".format(
**gyro_only))
rospy.loginfo(
"imu/gyroscope/raw: x: {x}, y: {y}, z: {z}".format(**gyro_raw))
rospy.loginfo("imu/magnetometer: North: %s" % north)
rospy.loginfo(
"imu/magnetometer/raw: x: {x}, y: {y}, z: {z}".format(
**compass))
rospy.loginfo(
"imu/orientation: p: {pitch}, r: {roll}, y: {yaw}".format(
**orientation))
rospy.loginfo(
"imu/orientation/degrees: p: {pitch}, r: {roll}, y: {yaw}".
format(**orientation_deg))
rospy.loginfo(
"imu/orientation/radians: p: {pitch}, r: {roll}, y: {yaw}".
format(**orientation_rad))
rospy.loginfo("imu/orientation/north: North: %s" % north)
self.imuPub.publish(imu_msg)
self.imuRawPub.publish(imu_raw_msg)
self.accelerometerPub.publish(accel_msg)
self.accelerometerPitchPub.publish(accel_pitch_msg)
self.accelerometerRollPub.publish(accel_roll_msg)
self.accelerometerYawPub.publish(accel_yaw_msg)
self.accelerometerRawPub.publish(accel_raw_msg)
self.accelerometerRawXPub.publish(accel_raw_x_msg)
self.accelerometerRawYPub.publish(accel_raw_y_msg)
self.accelerometerRawZPub.publish(accel_raw_z_msg)
self.gyroscopePub.publish(gyro_msg)
self.gyroscopePitchPub.publish(gyro_pitch_msg)
self.gyroscopeRollPub.publish(gyro_roll_msg)
self.gyroscopeYawPub.publish(gyro_yaw_msg)
self.gyroscopeRawPub.publish(gyro_raw_msg)
self.gyroscopeRawXPub.publish(gyro_raw_x_msg)
self.gyroscopeRawYPub.publish(gyro_raw_y_msg)
self.gyroscopeRawZPub.publish(gyro_raw_z_msg)
self.magnetometerPub.publish(north_msg)
self.magnetometerRawPub.publish(compass_msg)
self.orientationPub.publish(orientation_msg)
self.orientationDegreePub.publish(orientation_degree_msg)
self.orientationRadiansPub.publish(orientation_rad_msg)
self.orientationNorthPub.publish(north_msg)
self.rate.sleep()
