Skip to content
Snippets Groups Projects
Select Git revision
  • 59a22b099a3d52101cec6500aeddf4dd3fdcfd2a
  • master default
2 results

OrTests.kt

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