diff --git a/README.org b/README.org index 45e2585..cfdf11a 100644 --- a/README.org +++ b/README.org @@ -19,6 +19,17 @@ 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 +# 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: @@ -45,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!** -- [ ] 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 +- [x] Read from text file instead of serial or UDP - [ ] Other data collection methods: bluetooth? - [ ] Write tests, docstrings etc diff --git a/pyteapot.py b/pyteapot.py index d13ae5f..007df07 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,13 +25,13 @@ 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, useSerial, useQuat) else: - [yaw, pitch, roll] = read_data() + [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))) @@ -75,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', '') @@ -95,10 +82,10 @@ def cleanSerialBegin(): pass -def read_data(): +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: @@ -120,7 +107,7 @@ def read_data(): 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) @@ -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) + 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))