diff --git a/IPOS_Rawdata.py b/IPOS_Rawdata.py index 70f236f81dd0eef67c62d55e5149c14d7f1805f1..c956f4f35d0bd7d705d2edef30b7b6406a34049d 100644 --- a/IPOS_Rawdata.py +++ b/IPOS_Rawdata.py @@ -1 +1,146 @@ -pow(float(distance),2)-pow((position_3383[2]-height_tag),2) \ No newline at end of file +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) + diff --git a/sep/IPOSSensorValueTransmitter.py b/sep/IPOSSensorValueTransmitter.py index a429f91df336d512973b7c7ce9e9ca4c1cc6351c..56f02dc8540f94bc7331a4efc1fb8754e43944f5 100644 --- a/sep/IPOSSensorValueTransmitter.py +++ b/sep/IPOSSensorValueTransmitter.py @@ -26,9 +26,17 @@ def on_message(client, userdata, msg): print("Received message: " + msg.topic + " -> " + msg.payload.decode("utf-8")) class IPOSSensorValueTransmitter: - - def create_rawdata_msg(sensorId, listBeaconId, listDistance): - + __init__(self, mqttServer, mqttPort, sensorTyp): + # create the client + self.client = mqtt.Client() + self.client.on_connect = on_connect + self.client.on_message = on_message + self.client.connect(mqttServer, mqttPort) + self.sensorType = sensorTyp + + +class IPOSUwbSensorValueTransmitter(IPOSSensorValueTransmitter): + def send_rawdata_msg(client, sensorId, beaconId): timestamp = str(datetime.datetime.now().date()) timestamp += "T" timestamp += str(datetime.datetime.now().time()) @@ -54,8 +62,8 @@ class IPOSSensorValueTransmitter: wrapper_msg = {"uwbRawDataEvent" : [json_msg] } msg = json.dumps(wrapper_msg) - print(msg) - return msg + #print(msg) + self.client.publish(topic, msg) def create_position_msg(sensorId, x, y, z, accuracy, refSystemId, ori_x, ori_y, ori_z, ori_w): @@ -106,9 +114,11 @@ class IPOSSensorValueTransmitter: client.connect(mqttServer, mqttPort) return client - def sendMessage(client, message, topic): - client.publish(topic, message) +class IPOSUwbValueTransmitter(IPOSSensorValueTransmitter): + def __init__(): + + def diff --git a/sep/IPOS_Rawdata_sep.py b/sep/IPOS_Rawdata_sep.py index ba7ac92a3944ec3e4b56fc466b0e98ce06510350..0990930f7f2f7d126dd22d9a52876f66ce75a22b 100644 --- a/sep/IPOS_Rawdata_sep.py +++ b/sep/IPOS_Rawdata_sep.py @@ -78,7 +78,7 @@ def read_serial(number): return average(d1), average(d2), average(d3), average(d4) # create the client -client = connect_mqtt(mqttServer, mqttPort) +uwbValueTransmitter = IPOSUwbValueTransmitter.__init__(mqttServer, mqttPort,"UWB") # enable TLS #client.tls_set(tls_version=mqtt.ssl.PROTOCOL_TLS) @@ -95,8 +95,6 @@ while True: distance = read_serial(60) if (distance == [-1, -1, -1, -1]): continue - msg=create_rawdata_msg(sensorId, beaconId, distance) - - sendMessage(client, msg, topic) + uwbValueTransmitter.send_rawdata_msg(sensorId, beaconId, distance) time.sleep(1-((time.time() - starttime)%1))