update IPOS_Rawdata.py

IPOS_Rawdata.py

-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)
+

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 
         
         
         

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))