- Framework related functions separated.

IPOS_Rawdata.py

+pow(float(distance),2)-pow((position_3383[2]-height_tag),2)
\ No newline at end of file

sep/IPOSSensorValueTransmitter.py

+# -*- coding: utf-8 -*-
+"""
+Created on Wed Jan 26 09:52:38 2022
+
+@author: s3340992
+"""
+from error import *
+import math
+import numpy as np
+import logging
+import re
+import datetime
+import time
+import json
+import paho.mqtt.client as mqtt
+
+# 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"))
+
+class IPOSSensorValueTransmitter:
+
+    def create_rawdata_msg(sensorId, listBeaconId, listDistance):
+        
+        timestamp = str(datetime.datetime.now().date())
+        timestamp += "T"
+        timestamp += str(datetime.datetime.now().time())
+        timestamp += "+00:00"
+        #print(timestamp)
+        
+        #Check the length of list_beacon_id and list_distance, they must be the same
+        if listBeaconId.size() != listDistance.size():
+            raise RawDataSizeError
+        
+        distance_msg = {
+            listBeaconId[0] : listDistance[0],
+            }
+        
+        for i in range(len(listBeaconId)-1):
+            distance_msg[listBeaconId[i+1]] = listDistance[i+1]
+    
+        json_msg = {
+            "timestamp" : timestamp,
+            "sensorId" : sensorId,
+            "distances" : distance_msg
+            }
+        wrapper_msg = {"uwbRawDataEvent" : [json_msg]
+            }
+        msg = json.dumps(wrapper_msg)
+        print(msg)
+        return msg
+
+
+    def create_position_msg(sensorId, x, y, z, accuracy, refSystemId, ori_x, ori_y, ori_z, ori_w):
+        #print(d[0])
+        timestamp = str(datetime.datetime.now().date())
+        timestamp += "T"
+        timestamp += str(datetime.datetime.now().time())
+        timestamp += "+00:00"
+        #print(timestamp)
+    
+        point_msg = {
+            "x" : x,
+            "y" : y,
+            "z" : z
+            }
+    
+        position_msg = {
+            "refSystemId" : refSystemId,
+            "point" : point_msg,
+            "accuracy" : accuracy
+            }
+        orientation_msg = {
+            "x" : ori_x,
+            "y" : ori_y,
+            "z" : ori_z,
+            "w" : ori_w
+            }
+    
+        json_msg = {
+            "sensorId" : sensorId,
+            "position" : position_msg,
+            "orientation" : orientation_msg,
+            "lastPosUpdate" : timestamp
+            }
+        wrapper_msg = {"sensorPositionEvent" : [json_msg]
+            }
+        msg = json.dumps(wrapper_msg)
+        print(msg)
+        return msg
+    
+
+    
+    def connect_mqtt(mqttServer, mqttPort):
+        # create the client
+        client = mqtt.Client()
+        client.on_connect = on_connect
+        client.on_message = on_message
+        client.connect(mqttServer, mqttPort)
+        return client
+    
+    def sendMessage(client, message, topic):
+        client.publish(topic, message)
+
+        
+        
+        
+    

sep/IPOS_Rawdata_sep.py

+import serial
+import math
+import re
+import time
+from IPOSSensorValueTransmitter import connect_mqtt, create_rawdata_msg, sendMessage
+# 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"
+mqttServer = "192.168.0.143"
+mqttPort = 1883
+#MQTT_SERVER = "broker.hivemq.com"
+sensorId = "UWB1"
+topic = "usertopic/SensorEventWrapper"
+beaconId = ["3383", "885", "2934", "1107"]
+
+
+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)
+
+# create the client
+client = connect_mqtt(mqttServer, mqttPort)
+
+# 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:
+    distance = read_serial(60)
+    if (distance == [-1, -1, -1, -1]):
+        continue
+    msg=create_rawdata_msg(sensorId, beaconId, distance)
+    
+    sendMessage(client, msg, topic)
+    time.sleep(1-((time.time() - starttime)%1))
+

sep/error.py

+# -*- coding: utf-8 -*-
+"""
+Created on Wed Jan 26 15:44:00 2022
+
+@author: s3340992
+"""
+
+# define Python user-defined exceptions
+class Error(Exception):
+    """Base class for other exceptions"""
+    pass
+
+class RawDataSizeError(Error):
+    """Raised when the input raw data size is not consistent"""
+    pass
\ No newline at end of file