diff --git a/arduino_sensor_simulator/arduino_sensor_simulator_latency_tester/arduino_sensor_simulator_latency_tester.ino b/arduino_sensor_simulator/arduino_sensor_simulator_latency_tester/arduino_sensor_simulator_latency_tester.ino index 53387b8694c235972de4c2fc2bf41e582193fbae..9cff405b28cd36e148c6ae0d9cefc8b96191fbb6 100644 --- a/arduino_sensor_simulator/arduino_sensor_simulator_latency_tester/arduino_sensor_simulator_latency_tester.ino +++ b/arduino_sensor_simulator/arduino_sensor_simulator_latency_tester/arduino_sensor_simulator_latency_tester.ino @@ -1,6 +1,8 @@ int ctr = 0; +int ctr_limit = 50; /*5 seconds*/ + void setup() { - Serial.begin(9600); + Serial.begin(115200); } void generate_sensor_set1() @@ -13,7 +15,7 @@ void generate_sensor_set1() String s = "GE#"; for(i=0; i<64; i++){ r=0; - if(ctr==100) r=100; + if(ctr==ctr_limit) r=100; s += String(r); s += ","; } @@ -21,14 +23,14 @@ void generate_sensor_set1() s += "TOF#"; //TOF1 r=0; - if(ctr==100) r=100; + if(ctr==ctr_limit) r=100; s += String(r); s += ","; //TOF2 r=0; - if(ctr==100) r=100; + if(ctr==ctr_limit) r=100; s += String(r); s += ","; @@ -37,25 +39,25 @@ void generate_sensor_set1() //w q = 0.0; - if(ctr==100) q=2.0; + if(ctr==ctr_limit) q=2.0; s += String(q); s += ","; //x q = 0.0; - if(ctr==100) q=2.0; + if(ctr==ctr_limit) q=2.0; s += String(q); s += ","; //y q = 0.0; - if(ctr==100) q=2.0; + if(ctr==ctr_limit) q=2.0; s += String(q); s += ","; //z q = 0.0; - if(ctr==100) q=2.0; + if(ctr==ctr_limit) q=2.0; s += String(q); s += ","; Serial.println(""); @@ -71,7 +73,7 @@ void generate_sensor_set2() String s = "PR#"; for(i=0; i<60; i++){ r=0; - if(ctr==100) r=100; + if(ctr==ctr_limit) r=100; s += String(r); s += ","; } @@ -87,5 +89,5 @@ void loop() { generate_sensor_set1(); generate_sensor_set2(); delay(100); - if(ctr>100) ctr = 0; + if(ctr>ctr_limit) ctr = 0; } diff --git a/lib/NeoViki_UI_Python/src/NeoViki_UI_Python.py b/lib/NeoViki_UI_Python/src/NeoViki_UI_Python.py index 809ac5dbba185ef87ffeaa356b1abe4224e25481..8536a1392ad0bfe252b727fbf8f701540f8839e8 100644 --- a/lib/NeoViki_UI_Python/src/NeoViki_UI_Python.py +++ b/lib/NeoViki_UI_Python/src/NeoViki_UI_Python.py @@ -139,6 +139,7 @@ class WINDOW(UI_COMMON): self.resize = False self.title = "" self.handle = tk.Tk() + self.width = self.handle.winfo_reqwidth() self.height = self.handle.winfo_reqheight() diff --git a/src/main.py b/src/main.py index 19031e81c6e98e3d3179995071cd0c3191f9e6cb..b976f8ecf8f46181b95c8042370205b78fed8ac0 100644 --- a/src/main.py +++ b/src/main.py @@ -14,6 +14,11 @@ import time # python2 -m pip install pyserial import serial import serial.tools.list_ports +import threading +import time +import queue +from datetime import datetime + # Library Path sys.path.append('../lib/NeoViki_UI_Python/src') @@ -21,12 +26,16 @@ from NeoViki_UI_Python import * DEBUG = False serial_handle = None -sensor_readings = None +tstart_be = datetime.now() +tstart_fe = datetime.now() +sensor_queue=None start_x = 40 start_y = 40 timer_interval=1 +thread_be=None +thread_ui=None class COLOR_CODE: def __init__(self): @@ -327,8 +336,7 @@ def UI_LabelsDraw(): l.gotoxy(x, y) -def UI_GridEyeDraw(): - global sensor_readings +def UI_GridEyeDraw(sensor_readings): global ui_params grid_width = 30 @@ -393,8 +401,7 @@ def UI_GridEyeDraw(): sensor_readings.ge.available = False -def UI_PRDraw(): - global sensor_readings +def UI_PRDraw(sensor_readings): global ui_params grid_width = 30 @@ -477,8 +484,8 @@ def UI_PRDraw(): -def UI_Tof1Draw(): - global ui_params, sensor_readings +def UI_Tof1Draw(sensor_readings): + global ui_params # Return if sensor data is not available if sensor_readings.tof1.available == False: @@ -526,8 +533,8 @@ def UI_Tof1Draw(): sensor_readings.tof1.available = False -def UI_Tof2Draw(): - global ui_params, sensor_readings +def UI_Tof2Draw(sensor_readings): + global ui_params # Return if sensor data is not available if sensor_readings.tof2.available == False: @@ -579,8 +586,8 @@ def get_degree(): return random.randint(-360, 360) -def UI_IMUDraw(): - global ui_params, sensor_readings +def UI_IMUDraw(sensor_readings): + global ui_params # Return if sensor data is not available if sensor_readings.imu.available == False: return @@ -670,24 +677,32 @@ def Frontend_Routine(): ui_params.root.handle.gotoxy(80, 25) UI_LabelsDraw() - + sensor_readings=SENSOR_READINGS() # First time Make it True sensor_readings.tof1.available = True sensor_readings.tof2.available = True sensor_readings.ge.available = True sensor_readings.imu.available = True - UI_GridEyeDraw() - UI_PRDraw() - UI_Tof1Draw() - UI_Tof2Draw() - UI_IMUDraw() - ui_params.root.handle.timer(timer_interval, Backend_Routine) + UI_GridEyeDraw(sensor_readings) + UI_PRDraw(sensor_readings) + UI_Tof1Draw(sensor_readings) + UI_Tof2Draw(sensor_readings) + UI_IMUDraw(sensor_readings) + UI_Update_Thread() END(ui_params.root.handle) +def test(): + print("close") + print("close") + print("close") + print("close") + print("close") + print("close") + class SerialMonitor: def __init__(self): - sensor_readings = SENSOR_READINGS() + self.sensor_readings = SENSOR_READINGS() self.handle = None self.available = False self.connected = False @@ -716,7 +731,7 @@ class SerialMonitor: self.baudrate = baud_rate #Serial Buffer Size self.handle = serial.Serial(self.serial_port_str, self.baudrate) - self.handle.ReadBufferSize=10000 + #self.handle.ReadBufferSize=10000 self.handle.flushInput() except: print("error: serial connect -> " + serial_port_str) @@ -750,7 +765,6 @@ class SerialMonitor: return serial_data def parse_pressure(self, serial_data): - global sensor_readings s = str(serial_data) if DEBUG == True: print("PR(1)-->" + str(s)) e1 = s.split("PR#") @@ -764,16 +778,15 @@ class SerialMonitor: e3 = e2.split(",") if DEBUG == True: print("PR(4)-->" + str(e3)) - if sensor_readings.pr.available != True: + if self.sensor_readings.pr.available != True: for i in range(0, 60): - sensor_readings.pr.pressure[i] = int(e3[i]) + self.sensor_readings.pr.pressure[i] = int(e3[i]) - sensor_readings.pr.available = True + self.sensor_readings.pr.available = True - if DEBUG == True: print("PR(5)-->" + str(sensor_readings.pr.pressure)) + if DEBUG == True: print("PR(5)-->" + str(self.sensor_readings.pr.pressure)) def parse_tof_ge_imu(self, serial_data): - global sensor_readings s = str(serial_data) if DEBUG == True: print("GE(1)-->" + str(s)) @@ -788,13 +801,13 @@ class SerialMonitor: e3 = e2.split(",") if DEBUG == True: print("GE(4)-->" + str(e3)) - if sensor_readings.ge.available != True: + if self.sensor_readings.ge.available != True: for i in range(0, 64): - sensor_readings.ge.pixel[i] = int(e3[i]) + self.sensor_readings.ge.pixel[i] = int(e3[i]) - sensor_readings.ge.available = True + self.sensor_readings.ge.available = True - if DEBUG == True: print("GE(5)-->" + str(sensor_readings.ge.pixel)) + if DEBUG == True: print("GE(5)-->" + str(self.sensor_readings.ge.pixel)) if DEBUG == True: print("TOF(1)-->" + str(s)) e1 = s.split("TOF#") @@ -808,13 +821,13 @@ class SerialMonitor: if DEBUG == True: print("TOF(5) TOF1 READING -->" + str(e3[0])) if DEBUG == True: print("TOF(6) TOF2 READING -->" + str(e3[1])) - if sensor_readings.tof1.available != True: - sensor_readings.tof1.distance = int(float(e3[0])) - sensor_readings.tof1.available = True + if self.sensor_readings.tof1.available != True: + self.sensor_readings.tof1.distance = int(float(e3[0])) + self.sensor_readings.tof1.available = True - if sensor_readings.tof2.available != True: - sensor_readings.tof2.distance = int(float(e3[1])) - sensor_readings.tof2.available = True + if self.sensor_readings.tof2.available != True: + self.sensor_readings.tof2.distance = int(float(e3[1])) + self.sensor_readings.tof2.available = True if DEBUG == True: print("IMU(1)-->" + str(s)) e1 = s.split("IMU#") @@ -831,13 +844,13 @@ class SerialMonitor: if DEBUG == True: print("IMU(5.3)-->" + str(e3[2])) if DEBUG == True: print("IMU(5.4)-->" + str(e3[3])) - if sensor_readings.imu.available != True: - sensor_readings.imu.w = float(e3[0]) - sensor_readings.imu.x = float(e3[1]) - sensor_readings.imu.y = float(e3[2]) - sensor_readings.imu.z = float(e3[3]) - sensor_readings.imu.available = True - sensor_readings.imu.generate_euler_angles() + if self.sensor_readings.imu.available != True: + self.sensor_readings.imu.w = float(e3[0]) + self.sensor_readings.imu.x = float(e3[1]) + self.sensor_readings.imu.y = float(e3[2]) + self.sensor_readings.imu.z = float(e3[3]) + self.sensor_readings.imu.available = True + self.sensor_readings.imu.generate_euler_angles() def parse(self, serial_data): s = str(serial_data) @@ -850,20 +863,42 @@ class SerialMonitor: self.parse_tof_ge_imu(serial_data) def UI_Update(): - global ui_params - global sensor_readings - UI_GridEyeDraw() - UI_PRDraw() - UI_Tof1Draw() - UI_Tof2Draw() - UI_IMUDraw() - # ui_params.root.handle.update() + global ui_params, sensor_queue + if sensor_queue.empty(): + print_frontend_latency() + #ui_params.root.handle.timer(0, UI_Update) + return + sensor_readings = sensor_queue.get() + UI_GridEyeDraw(sensor_readings) + UI_PRDraw(sensor_readings) + UI_Tof1Draw(sensor_readings) + UI_Tof2Draw(sensor_readings) + UI_IMUDraw(sensor_readings) + ui_params.root.handle.update() + sensor_queue.task_done() + #ui_params.root.handle.timer(0, UI_Update) + print_frontend_latency() + +def print_backend_latency(): + global tstart_be + tend = datetime.now() + tdiff = tend - tstart_be + tdiff_ms = (tdiff.microseconds / 1000) + print("BE : "+str(tdiff_ms)) + tstart_be=tend + +def print_frontend_latency(): + global tstart_fe + tend = datetime.now() + tdiff = tend - tstart_fe + tdiff_ms = (tdiff.microseconds / 1000) + print("FE : "+str(tdiff_ms)) + tstart_fe=tend def Backend_Routine(): - global sensor_readings + global sensor_queue global serial_port, baud_rate - ui_params.root.handle.timer(timer_interval, Backend_Routine) global serial_handle data_raw = None ui_update = True @@ -871,19 +906,46 @@ def Backend_Routine(): if serial_handle.connected == False: serial_handle.connect(serial_port, baud_rate) if serial_handle.connected == False: + print_backend_latency() return data_raw = serial_handle.read() serial_handle.parse(data_raw) - UI_Update() + sensor_queue.put(serial_handle.sensor_readings) + print_backend_latency() + + +def Backend_Loop(): + global sensor_queue + while True: + Backend_Routine() + time.sleep(0.00001) +def UI_Update_Loop(): + while True: + UI_Update() + time.sleep(0.001) + +def UI_Update_Thread(): + global thread_ui + thread_ui = threading.Thread(target=UI_Update_Loop) + thread_ui.start() + +def Backend_Routine_Thread(): + global thread_be + thread_be = threading.Thread(target=Backend_Loop) + thread_be.start() def MAIN(): - global serial_handle, sensor_readings - sensor_readings = SENSOR_READINGS() + global serial_handle, sensor_queue, thread_be, thread_ui + sensor_queue = queue.Queue() serial_handle = SerialMonitor() + Backend_Routine_Thread() Frontend_Routine() - + print("FE Closed") + thread_ui.exit() + thread_be.exit() + sensor_queue.join() def ARG_CHECK(): global serial_port, baud_rate