Select Git revision
IPOS_Rawdata.py 4.50 KiB
import serial
import math
import numpy as np
import logging
import re
import datetime
import time
import json
import paho.mqtt.client as mqtt
from random import randrange
# setups
position_3383 = [1.25,5.89,1.06]
position_885 = [0,0,0.97]
position_2934 = [4.27,1.33,1]
position_1107 = [4.24,5.83,1.05]
height_tag = 0
the_hostname = "UWB_rasp"
MQTT_SERVER = "192.168.0.143"
#MQTT_SERVER = "broker.hivemq.com"
MQTT_PATH = "usertopic/SensorEventWrapper"
ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1) # open serial port
print(ser.name) # check which port was really used
def extractNumbersFromLine(line):
return re.findall(r'-?\d+\.?\d*',line)
def get_distance(number_found):
return number_found[1], number_found[3]
def average(lst):
return sum(lst) / len(lst)
def read_serial(number):
d1 = []
d2 = []
d3 = []
d4 = []
print("read started")
with serial.Serial('/dev/ttyUSB0', 115200, timeout=1) as ser:
for x in range(number):
line = ser.readline() # read a '\n' terminated line
#print(len(line))
if (len(line) <= 30):
print("reseting serial...")
ser.close()
ser.open()
line = ser.readline()
#print(line)
number_found = extractNumbersFromLine(line.decode("utf-8"))
if(len(number_found)>=4):
tag, distance = get_distance(number_found)
#print(tag, distance)
if (tag == "3383"):
distance_xy = pow(float(distance),2)-pow((position_3383[2]-height_tag),2)
if (distance_xy >= 0):
distance = math.sqrt(distance_xy)
d1.append(float(distance))
elif (tag == "885"):
distance_xy = pow(float(distance),2)-pow((position_885[2]-height_tag),2)
if (distance_xy >= 0):
distance = math.sqrt(distance_xy)
d2.append(float(distance))
elif (tag == "2934"):
distance_xy = pow(float(distance),2)-pow((position_2934[2]-height_tag),2)
if (distance_xy >= 0):
distance = math.sqrt(distance_xy)
d3.append(float(distance))
elif (tag == "1107"):
distance_xy = pow(float(distance),2)-pow((position_1107[2]-height_tag),2)
if (distance_xy >= 0):
distance = math.sqrt(distance_xy)
d4.append(float(distance))
if (len(d1) == 0 or len(d2) == 0 or len(d3) == 0 or len(d4) == 0):
print ("sth went wrong")
return [-1, -1, -1, -1]
return average(d1), average(d2), average(d3), average(d4)
def create_msg(d):
#print(d[0])
timestamp = str(datetime.datetime.now().date())
timestamp += "T"
timestamp += str(datetime.datetime.now().time())
timestamp += "+00:00"
#print(timestamp)
distance_msg = {
"3383" : d[0],
"885" : d[1],
"2934" : d[2],
"1107" : d[3]
}
json_msg = {
"timestamp" : timestamp,
"sensorId" : "UWB_1",
"distances" : distance_msg
}
wrapper_msg = {"uwbRawDataEvent" : [json_msg]
}
msg = json.dumps(wrapper_msg)
print(msg)
return msg
# The callback for when the client receives a CONNACK response from the server.
def on_connect(client, userdata, flags, rc):
if rc == 0:
print("Connected successfully")
else:
print("Connect returned result code: " + str(rc))
# The callback for when a PUBLISH message is received from the server.
def on_message(client, userdata, msg):
print("Received message: " + msg.topic + " -> " + msg.payload.decode("utf-8"))
# create the client
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
# enable TLS
#client.tls_set(tls_version=mqtt.ssl.PROTOCOL_TLS)
#client.username_pw_set("phone", "IPOSframework123")
client.connect(MQTT_SERVER, 1883)
#client.loop_start()
#client.subscribe(MQTT_PATH)
starttime = time.time()
line = ser.readline()
while True:
distances = read_serial(60)
if (distances == [-1, -1, -1, -1]):
continue
msg=create_msg(distances)
client.publish(MQTT_PATH, msg)
time.sleep(1-((time.time() - starttime)%1))
#client.loop_forever()
#publish.single(MQTT_PATH, msg, hostname=MQTT_SERVER, port=8884, auth={'username':"UWB_rasp", 'password':"IPOSframwork123"}, client_id=the_hostname)