From fd00e5f4819a9aa631ce2189fb51707967182b05 Mon Sep 17 00:00:00 2001 From: Sandeep Menon Date: Fri, 4 Mar 2022 22:30:54 +0530 Subject: [PATCH 1/6] added argparse interface --- README.org | 2 +- pyteapot.py | 50 +++++++++++++++++++++++++++++++------------------- 2 files changed, 32 insertions(+), 20 deletions(-) diff --git a/README.org b/README.org index 45e2585..ff3766f 100644 --- a/README.org +++ b/README.org @@ -47,7 +47,7 @@ Each of these must be on separate lines (or should have a '\n' at the end of the * TODO Todos - [x] Receive data over WiFi instead of serial. - **Done!** -- [ ] Add a nice [[https://docs.python.org/3/library/argparse.html][ ~argparse~ ]] interface instead of requiring the user to change variables in the script. Include example usage in docstring (a la [[https://tldr.sh/][tldr]] or [[http://bropages.org][bro]]) +- [x] Add a nice [[https://docs.python.org/3/library/argparse.html][ ~argparse~ ]] interface instead of requiring the user to change variables in the script. Include example usage in docstring (a la [[https://tldr.sh/][tldr]] or [[http://bropages.org][bro]]) - [ ] Add some keyboard support, eg. pausing / resuming the visualization with spacebar etc - [ ] Add optional support for x, y, z too. Also, multiple simultaneous viewports (eg. to compare with ground truth from MoCap) - [ ] Read from text file instead of serial or UDP diff --git a/pyteapot.py b/pyteapot.py index d13ae5f..a866ea5 100644 --- a/pyteapot.py +++ b/pyteapot.py @@ -8,23 +8,10 @@ from OpenGL.GL import * from OpenGL.GLU import * from pygame.locals import * +import argparse -useSerial = False # set true for using serial for data transmission, false for wifi -useQuat = False # set true for using quaternions, false for using y,p,r angles -if(useSerial): - import serial - ser = serial.Serial('/dev/ttyUSB0', 38400) -else: - import socket - - UDP_IP = "0.0.0.0" - UDP_PORT = 5005 - sock = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM) # UDP - sock.bind((UDP_IP, UDP_PORT)) - -def main(): +def main(useSerial, useQuat, ser, sock): video_flags = OPENGL | DOUBLEBUF pygame.init() screen = pygame.display.set_mode((640, 480), video_flags) @@ -38,9 +25,9 @@ def main(): if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE): break if(useQuat): - [w, nx, ny, nz] = read_data() + [w, nx, ny, nz] = read_data(ser, sock) else: - [yaw, pitch, roll] = read_data() + [yaw, pitch, roll] = read_data(ser, sock) if(useQuat): draw(w, nx, ny, nz) else: @@ -95,7 +82,7 @@ def cleanSerialBegin(): pass -def read_data(): +def read_data(ser, sock): if(useSerial): ser.reset_input_buffer() cleanSerialBegin() @@ -200,4 +187,29 @@ def quat_to_ypr(q): if __name__ == '__main__': - main() + # parse command line + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--useSerial', type=bool, default=False, help='set true for using serial for data transmission, false for wifi') + parser.add_argument('--useQuat', type=bool, default=False, help='set true for using quaternions, false for using y,p,r angles') + parser.add_argument('--port', type=str, default='/dev/ttyUSB0', help='serial port') + parser.add_argument('--udp_id', type=str, default="0.0.0.0") + parser.add_argument('--udp_port', type=int, default=5005) + args = parser.parse_args() + + useSerial = args.useSerial + useQuat = args.useQuat + + if(useSerial): + import serial + ser = serial.Serial(args.port, 38400) + else: + import socket + + UDP_IP = args.udp_id + UDP_PORT = args.udp_port + sock = socket.socket(socket.AF_INET, # Internet + socket.SOCK_DGRAM) # UDP + sock.bind((UDP_IP, UDP_PORT)) + + main(useSerial, useQuat, ser, sock) + From e6139cd69e04f5ced6a3acafbbaf5d8b509406b1 Mon Sep 17 00:00:00 2001 From: Sandeep Menon Date: Fri, 4 Mar 2022 22:44:32 +0530 Subject: [PATCH 2/6] example usage in docstring --- README.org | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/README.org b/README.org index ff3766f..29400e7 100644 --- a/README.org +++ b/README.org @@ -20,6 +20,15 @@ Most of the code is self-explanatory. However some modifications might be requir - Set =useQuat= to =True= if receiving *quaternions* over serial or WiFi and =False= if receiving *Euler angles*. - If receiving quaternions over serial or wifi, the declination at the particular location should to be updated in =quat_to_ypr(q)= function to get correct yaw angles printed *on screen*. (The cube rotation is not dependent on this and will still work fine otherwise) +Examples: +``` +# using serial stream and quaternion representation +python pyteapot.py --useSerial True --useQuat True --port /dev/ttyUSB0 + +# using udp stream and euler representation +python pyteapot.py --useSerial False --useQuat False --udp_id 0.0.0.0 --udp_port 5005 +``` + * String passed over Serial or Wifi To use this module, the data received over serial or udp port should be in the format specified below: - First quaternion value should be between two =w= s From b329137460ef8e24b87cb9f10bfc77fde4d19d8a Mon Sep 17 00:00:00 2001 From: Sandeep Menon Date: Fri, 4 Mar 2022 22:46:24 +0530 Subject: [PATCH 3/6] using #+END_EXAMPLE for highlights --- README.org | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.org b/README.org index 29400e7..9dd3b0d 100644 --- a/README.org +++ b/README.org @@ -21,13 +21,14 @@ Most of the code is self-explanatory. However some modifications might be requir - If receiving quaternions over serial or wifi, the declination at the particular location should to be updated in =quat_to_ypr(q)= function to get correct yaw angles printed *on screen*. (The cube rotation is not dependent on this and will still work fine otherwise) Examples: -``` +#+BEGIN_EXAMPLE # using serial stream and quaternion representation python pyteapot.py --useSerial True --useQuat True --port /dev/ttyUSB0 # using udp stream and euler representation python pyteapot.py --useSerial False --useQuat False --udp_id 0.0.0.0 --udp_port 5005 -``` +#+END_EXAMPLE + * String passed over Serial or Wifi To use this module, the data received over serial or udp port should be in the format specified below: From 76c674d4eb8977ccfe393eb0850867f35d3a6158 Mon Sep 17 00:00:00 2001 From: Sandeep Menon Date: Fri, 4 Mar 2022 23:01:23 +0530 Subject: [PATCH 4/6] passing use serial and quaternion to draw and read_data --- pyteapot.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pyteapot.py b/pyteapot.py index a866ea5..d182e0a 100644 --- a/pyteapot.py +++ b/pyteapot.py @@ -25,13 +25,13 @@ def main(useSerial, useQuat, ser, sock): if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE): break if(useQuat): - [w, nx, ny, nz] = read_data(ser, sock) + [w, nx, ny, nz] = read_data(ser, sock, useSerial, useQuat) else: - [yaw, pitch, roll] = read_data(ser, sock) + [yaw, pitch, roll] = read_data(ser, sock, useSerial, useQuat) if(useQuat): - draw(w, nx, ny, nz) + draw(w, nx, ny, nz, useQuat) else: - draw(1, yaw, pitch, roll) + draw(1, yaw, pitch, roll, useQuat) pygame.display.flip() frames += 1 print("fps: %d" % ((frames*1000)/(pygame.time.get_ticks()-ticks))) @@ -82,7 +82,7 @@ def cleanSerialBegin(): pass -def read_data(ser, sock): +def read_data(ser, sock, useSerial, useQuat): if(useSerial): ser.reset_input_buffer() cleanSerialBegin() @@ -107,7 +107,7 @@ def read_data(ser, sock): return [yaw, pitch, roll] -def draw(w, nx, ny, nz): +def draw(w, nx, ny, nz, useQuat): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) glLoadIdentity() glTranslatef(0, 0.0, -7.0) From 42a9b9bd392bb077bb6b4fb42d49832d75ff4360 Mon Sep 17 00:00:00 2001 From: Sandeep Menon Date: Fri, 4 Mar 2022 23:02:51 +0530 Subject: [PATCH 5/6] useQuat in cleanSerialBegin --- pyteapot.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pyteapot.py b/pyteapot.py index d182e0a..007df07 100644 --- a/pyteapot.py +++ b/pyteapot.py @@ -62,7 +62,7 @@ def init(): glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST) -def cleanSerialBegin(): +def cleanSerialBegin(useQuat): if(useQuat): try: line = ser.readline().decode('UTF-8').replace('\n', '') @@ -85,7 +85,7 @@ def cleanSerialBegin(): def read_data(ser, sock, useSerial, useQuat): if(useSerial): ser.reset_input_buffer() - cleanSerialBegin() + cleanSerialBegin(useQuat) line = ser.readline().decode('UTF-8').replace('\n', '') print(line) else: From 1414a5e761c6cf783f108a6ea87c26c4a3790324 Mon Sep 17 00:00:00 2001 From: Sandeep Menon Date: Fri, 4 Mar 2022 23:29:47 +0530 Subject: [PATCH 6/6] added visualisation from file --- README.org | 18 ++++++++++++-- pyteapot_from_file.py | 55 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 71 insertions(+), 2 deletions(-) create mode 100644 pyteapot_from_file.py diff --git a/README.org b/README.org index 9dd3b0d..cfdf11a 100644 --- a/README.org +++ b/README.org @@ -19,6 +19,7 @@ Most of the code is self-explanatory. However some modifications might be requir - Set udp port by changing the variable =UDP_PORT=, if using wifi for data transmission. - Set =useQuat= to =True= if receiving *quaternions* over serial or WiFi and =False= if receiving *Euler angles*. - If receiving quaternions over serial or wifi, the declination at the particular location should to be updated in =quat_to_ypr(q)= function to get correct yaw angles printed *on screen*. (The cube rotation is not dependent on this and will still work fine otherwise) +- See String passed from file for details regarding visualization from a file Examples: #+BEGIN_EXAMPLE @@ -55,11 +56,24 @@ y168.8099yp12.7914pr-11.8401r Each of these must be on separate lines (or should have a '\n' at the end of the string). Other data may also be passed over Serial or Wifi, provided that none of the characters =w=, =a=, =b=, =c=, =y=, =p=, =r= are passed (for example, =somethingw0.09wa-0.12ab-0.09bc0.98cy168.8099yp12.7914pr-11.8401rsomethingelse= is valid but =somedataw0.09wa-0.12ab-0.09bc0.98cy168.8099yp12.7914pr-11.8401ranotherstring= is not since it has the characters =a= and =r=) -* TODO Todos +* String passed from file +To use this module, the data should be stored in a file and the file name should be passed as an argument to the module. The file should contain the data with values separated by any delimeter. +The data can be quarternion or euler angles. While running the script =pyteapot_from_file.py=, you need to pass the column ids for the quarternion/euler angles so we can read appropriate values from the file. + +Examples +#+BEGIN_EXAMPLE +# using quarternion representation reading from csv file with quarternions in the 4,5,6,7 indexed columns (zero indexed) +python pyteapot_from_file.py --useQuat True --text_file_path /home/menonsandu/Downloads/dataset-corridor1_512_16/mav0/mocap0/data.csv --column_ids 4,5,6,7 --delimiter ',' + +# using euler representation reading from csv file with euler angles in the 1,2,3 indexed columns (zero indexed) +python pyteapot_from_file.py --useQuat False --text_file_path /home/menonsandu/Downloads/dataset-corridor1_512_16/mav0/imu0/data.csv --column_ids 1,2,3 --delimiter ',' +#+END_EXAMPLE + +* Todos - [x] Receive data over WiFi instead of serial. - **Done!** - [x] Add a nice [[https://docs.python.org/3/library/argparse.html][ ~argparse~ ]] interface instead of requiring the user to change variables in the script. Include example usage in docstring (a la [[https://tldr.sh/][tldr]] or [[http://bropages.org][bro]]) - [ ] Add some keyboard support, eg. pausing / resuming the visualization with spacebar etc - [ ] Add optional support for x, y, z too. Also, multiple simultaneous viewports (eg. to compare with ground truth from MoCap) -- [ ] Read from text file instead of serial or UDP +- [x] Read from text file instead of serial or UDP - [ ] Other data collection methods: bluetooth? - [ ] Write tests, docstrings etc diff --git a/pyteapot_from_file.py b/pyteapot_from_file.py new file mode 100644 index 0000000..06ed95a --- /dev/null +++ b/pyteapot_from_file.py @@ -0,0 +1,55 @@ +import os +import pygame +from OpenGL.GL import * +from OpenGL.GLU import * +from pygame.locals import * +import argparse +from pyteapot import resizewin, init, draw + +def visualize_quarternion_from_csv(file_path, useQuat, column_ids, delimiter, skip_header=True): + video_flags = OPENGL | DOUBLEBUF + pygame.init() + screen = pygame.display.set_mode((640, 480), video_flags) + pygame.display.set_caption("PyTeapot IMU orientation visualization") + resizewin(640, 480) + init() + ticks = pygame.time.get_ticks() + frames = 0 + + with open(file_path) as f: + lines = f.readlines() + for line in lines: + if(skip_header): + skip_header = False + continue + if(useQuat): + [w, nx, ny, nz] = [float(line.split(delimiter)[id]) for id in column_ids] + draw(w, nx, ny, nz, useQuat) + else: + [yaw, pitch, roll] = [float(line.split(delimiter)[id]) for id in column_ids] + draw(1, yaw, pitch, roll, useQuat) + pygame.display.flip() + frames += 1 + print("fps: %d" % ((frames*1000)/(pygame.time.get_ticks()-ticks))) + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--useQuat', type=bool, default=False, help='set true for using quaternions, false for using y,p,r angles') + parser.add_argument('--text_file_path', type=str, default='data.csv', help='text file name') + parser.add_argument('--delimiter', type=str, default=',', help='delimiter for file') + parser.add_argument('--column_ids', type=str, default='1,2,3,4', help='quarternion/euler column indices in the file') + args = parser.parse_args() + + useQuat = args.useQuat + file_path = args.text_file_path + delimiter = args.delimiter + column_ids = [int(id) for id in args.column_ids.split(',')] + if useQuat and len(column_ids) != 4: + raise Exception('Quaternion column indices should be 4. Representing qw, qx, qy, qz respectively') + if not useQuat and len(column_ids) != 3: + raise Exception('Euler column indices should be 3. Representing yaw, pitch, roll respectively') + + if os.path.exists(file_path): + visualize_quarternion_from_csv(file_path, useQuat, column_ids, delimiter, skip_header=True) + else: + raise FileNotFoundError('File not found: {}'.format(file_path))