From b91e595e188f4b1692e7a7a4200eee90bcde0cdf Mon Sep 17 00:00:00 2001 From: Mason Vogt <91571536+mvog2501@users.noreply.github.com> Date: Thu, 17 Nov 2022 12:58:52 -0600 Subject: [PATCH 01/10] Added framework for different position definitions --- src/solver.py | 36 +++++++++++++++++++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) diff --git a/src/solver.py b/src/solver.py index eadafb1..fa92b7c 100644 --- a/src/solver.py +++ b/src/solver.py @@ -57,7 +57,41 @@ def __init__(self, environment_dict): if not environment_dict['tags']: logger.error('No tags defined! Quitting') raise AssertionError('No tags defined in environment JSON') - self.tags_dict = {tag['id']: tag for tag in environment_dict['tags']} + # self.tags_dict = {tag['id']: tag for tag in environment_dict['tags']} + + self.tags_dict = {} + for tag in environment_dict['tags']: + # Extract the ID + tag_id = tag['id'] # Try it with the old style + if tag_id is None: + tag_id = tag['ID'] # Try it with WPILib style + + # Extract the position of the tag into a transform matrix + pose = tag['pose'] # All ways of writing it must be in "pose" : {} + + matrix = pose.get('matrix') + if matrix is not None: + self.tags_dict[tag_id] = matrix + continue + + translation = pose['translation'] + rotation = pose['rotation'] + + # See if it is a quaternion rotation + quaternion = rotation.get('quaternion') + if quaternion is not None: + # Convert to matrix + # FIXME + continue + + # If it make it here, it must be Pitch, Yaw, Roll + pitch = rotation['pitch'] + yaw = rotation['yaw'] + roll = rotation['roll'] + # Convert to matrix + # FIXME + + self.tag_family = environment_dict['tag_family'] if self.tag_family != "16h5": From 144bb244ef3a3940848b421edba2ee3651d4bf2d Mon Sep 17 00:00:00 2001 From: Mason Vogt <91571536+mvog2501@users.noreply.github.com> Date: Thu, 17 Nov 2022 23:51:56 -0600 Subject: [PATCH 02/10] Add different ways to define positions of tags (untested) --- environment.json | 69 ++++++++++++++++++++++++++++++++++------- requirements.txt | 1 + src/solver.py | 18 ++++++++--- src/transform_matrix.py | 49 +++++++++++++++++++++++++++++ 4 files changed, 121 insertions(+), 16 deletions(-) create mode 100644 src/transform_matrix.py diff --git a/environment.json b/environment.json index b1c6286..ccae0eb 100644 --- a/environment.json +++ b/environment.json @@ -5,23 +5,68 @@ { "size": 0.173, "id": 1, - "transform": [ - [1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ] + "pose": { + "matrix": [ + [1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1] + ] + } }, { "size": 0.165, "id": 2, - "transform": [ - [1, 0, 0, 0.2], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ] + "pose": { + "matrix": [ + [1, 0, 0, 0.2], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1] + ] + } + }, + + { + "size" : 0.165, + "id": 3, + "pose": { + "translation": { + "x": 1, + "y": 0, + "z": 0 + }, + + "rotation": { + "quaternion" : { + "W" : 0.7071067811865476, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.7071067811865475 + } + } + + } + }, + + { + "size" : 0.165, + "id": 4, + "pose": { + "translation": { + "x": 1, + "y": 0, + "z": 0 + }, + + "rotation": { + "pitch" : 3.1415, + "yaw": 0, + "roll": 0 + } + + } } ] } diff --git a/requirements.txt b/requirements.txt index facd2bd..ac191d9 100644 --- a/requirements.txt +++ b/requirements.txt @@ -3,3 +3,4 @@ numpy==1.22.3 opencv-contrib-python==4.5.5.64 pynetworktables==2021.0.0 imutils==0.5.4 +transformations==2022.9.26 \ No newline at end of file diff --git a/src/solver.py b/src/solver.py index fa92b7c..f5bf5dc 100644 --- a/src/solver.py +++ b/src/solver.py @@ -1,6 +1,8 @@ # Solves for robot position based on results of found tags import numpy as np from main import logger +from transform_matrix import * +from transformations import * # TODO-Ryan: Finish/Fix @@ -80,16 +82,24 @@ def __init__(self, environment_dict): # See if it is a quaternion rotation quaternion = rotation.get('quaternion') if quaternion is not None: - # Convert to matrix - # FIXME + w = quaternion['W'] # WPILib standard + x = quaternion['X'] # WPILib standard + y = quaternion['Y'] # WPILib standard + z = quaternion['Z'] # WPILib standard + matrix = quaternion_matrix([w,x,y,z]) + + + matrix = apply_translation(matrix, translation['x'], translation['y'], translation['z']) + self.tags_dict[tag_id] = matrix continue # If it make it here, it must be Pitch, Yaw, Roll pitch = rotation['pitch'] yaw = rotation['yaw'] roll = rotation['roll'] - # Convert to matrix - # FIXME + rotation_matrix = euler_to_matrix(pitch, yaw, roll) + matrix = apply_translation(rotation_matrix, translation['x'], translation['y'], translation['z']) + self.tags_dict[tag_id] = matrix self.tag_family = environment_dict['tag_family'] diff --git a/src/transform_matrix.py b/src/transform_matrix.py new file mode 100644 index 0000000..92f67c4 --- /dev/null +++ b/src/transform_matrix.py @@ -0,0 +1,49 @@ +import numpy as np +import math + +def euler_to_matrix(pitch, yaw, roll): + # x is pitch + # y is yaw + # z is roll + + # Do sin an cosine for all axis + s_pitch = math.sin(pitch) + c_pitch = math.cos(pitch) + + s_yaw = math.sin(yaw) + c_yaw = math.cos(yaw) + + s_roll = math.sin(roll) + c_roll = math.cos(roll) + + # Initial matrix to edit + x = np.array( + [[1, 0, 0, 0], + [0, c_pitch, -s_pitch, 0], + [0, s_pitch, c_pitch, 0], + [0, 0, 0, 1]] + ) + + y = np.array( + [[c_yaw, 0, s_yaw, 0], + [0, 1, 0, 0], + [-s_yaw, 0, c_yaw, 0], + [0, 0, 0, 1]] + ) + + z = np.array( + [[c_roll, -s_roll, 0, 0], + [s_roll, c_roll, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]] + ) + + # Combine indevidual matricies + return np.dot(z, np.dot(y,x)) + +def apply_translation(matrix, x, y, z): + matrix[0][3] = x + matrix[1][3] = y + matrix[2][3] = z + return matrix + From 61e82b06adb127c443290190f0da66710beb9cff Mon Sep 17 00:00:00 2001 From: Mason Vogt <91571536+mvog2501@users.noreply.github.com> Date: Fri, 18 Nov 2022 09:30:05 -0600 Subject: [PATCH 03/10] Import specific functions instead of * --- src/solver.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/solver.py b/src/solver.py index f5bf5dc..c5d4a1f 100644 --- a/src/solver.py +++ b/src/solver.py @@ -1,8 +1,8 @@ # Solves for robot position based on results of found tags import numpy as np from main import logger -from transform_matrix import * -from transformations import * +from transform_matrix import euler_to_matrix, apply_translation +from transformations import quaternion_matrix # TODO-Ryan: Finish/Fix From 4687de6d25bde33a3156cefa3043649d2331fcc1 Mon Sep 17 00:00:00 2001 From: rmheuer <91571536+mvog2501@users.noreply.github.com> Date: Wed, 21 Dec 2022 11:28:38 -0600 Subject: [PATCH 04/10] Updated JSONs to match current setup --- cameras.json | 4 ++-- environment.json | 58 +++++------------------------------------------- 2 files changed, 7 insertions(+), 55 deletions(-) diff --git a/cameras.json b/cameras.json index fa525cc..ee3191e 100644 --- a/cameras.json +++ b/cameras.json @@ -4,9 +4,9 @@ { "name": "USB Webcam", "type": "Small_USB_Camera", - "port": 2, + "port": 0, "robot_pose": [ - [1, 0, 0, 0.5], + [1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1] diff --git a/environment.json b/environment.json index 957833b..cd9ea1a 100644 --- a/environment.json +++ b/environment.json @@ -1,63 +1,15 @@ { - "tag_family" : "tag16h5", + "tag_family" : "36h11", "tags" : [ - { - "size": 0.173, - "id": 1, - "pose": { - "matrix": [ - [1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ] - } - }, - - { - "size": 0.165, - "id": 2, - "pose": { - "matrix": [ - [1, 0, 0, 0.2], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ] - } - }, - - { - "size" : 0.165, - "id": 3, - "pose": { - "translation": { - "x": 1, - "y": 0, - "z": 0 - }, - - "rotation": { - "quaternion" : { - "W" : 0.7071067811865476, - "X" : 0.0, - "Y" : 0.0, - "Z" : 0.7071067811865475 - } - } - - } - }, - { - "size" : 0.165, - "id": 4, + "size" : 0.175, + "id": 9, "pose": { "translation": { - "x": 1, + "x": 3.594, "y": 0, - "z": 0 + "z": 0.2 }, "rotation": { From 77584e9e343e1aa2090bddc4845eeff52ca3dc49 Mon Sep 17 00:00:00 2001 From: Mason Vogt <91571536+mvog2501@users.noreply.github.com> Date: Wed, 21 Dec 2022 14:27:46 -0600 Subject: [PATCH 05/10] Fixed crashes caused by new formating --- src/solver.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/solver.py b/src/solver.py index 7a482b3..3e8975b 100644 --- a/src/solver.py +++ b/src/solver.py @@ -68,12 +68,17 @@ def __init__(self, environment_dict): if tag_id is None: tag_id = tag['ID'] # Try it with WPILib style + self.tags_dict[tag_id] = { + 'size' : tag['size'], + 'pose': None + } + # Extract the position of the tag into a transform matrix pose = tag['pose'] # All ways of writing it must be in "pose" : {} matrix = pose.get('matrix') if matrix is not None: - self.tags_dict[tag_id] = matrix + self.tags_dict[tag_id]['pose'] = matrix continue translation = pose['translation'] @@ -90,7 +95,7 @@ def __init__(self, environment_dict): matrix = apply_translation(matrix, translation['x'], translation['y'], translation['z']) - self.tags_dict[tag_id] = matrix + self.tags_dict[tag_id]['pose'] = matrix continue # If it make it here, it must be Pitch, Yaw, Roll @@ -99,7 +104,7 @@ def __init__(self, environment_dict): roll = rotation['roll'] rotation_matrix = euler_to_matrix(pitch, yaw, roll) matrix = apply_translation(rotation_matrix, translation['x'], translation['y'], translation['z']) - self.tags_dict[tag_id] = matrix + self.tags_dict[tag_id]['pose'] = matrix self.tag_family = environment_dict['tag_family'] @@ -127,11 +132,11 @@ def solve(self, detection_poses): # Get the info for the tag tag_dict = self.tags_dict.get(tag_id) - if not tag_dict: + if tag_dict is None: logger.warning(f"Found a tag that isn't defined in environment. ID: {tag_id}") continue - tag_pose = tag_dict['transform'] + tag_pose = tag_dict['pose'] # Convert to numpy arrarys for math estimated_pose = np.array(estimated_pose) From d9a5feccecf874fc96133b36fff872c2a72f10e5 Mon Sep 17 00:00:00 2001 From: rmheuer <63077980+rmheuer@users.noreply.github.com> Date: Thu, 22 Dec 2022 10:36:06 -0600 Subject: [PATCH 06/10] Fix ShuffleLog api tag sending --- src/main.py | 4 ++-- src/messenger.py | 8 ++++++-- src/shufflelog_api.py | 22 +++++++++++++++++++--- 3 files changed, 27 insertions(+), 7 deletions(-) diff --git a/src/main.py b/src/main.py index 12d6b8d..fee03e1 100644 --- a/src/main.py +++ b/src/main.py @@ -100,9 +100,9 @@ def main(): 'host': 'localhost', 'port': 5805, 'name': 'TagTracker', - 'mute_errors': True + 'mute_errors': False } - api = ShuffleLogAPI(messenger_params, environment['tags'], cameras['cameras']) + api = ShuffleLogAPI(messenger_params, solver.tags_dict, cameras['cameras']) # Main loop, run all the time like limelight diff --git a/src/messenger.py b/src/messenger.py index 2cc62d7..9fa7d0a 100644 --- a/src/messenger.py +++ b/src/messenger.py @@ -13,7 +13,7 @@ import threading import struct import select - +import traceback class MessageBuilder: """ @@ -528,7 +528,11 @@ def _read_message(self): message_data = self._read(data_len) for handler in self.handlers: - handler.handle(message_type, message_data) + try: + handler.handle(message_type, message_data) + except Exception as e: + print("Exception in message handler:") + print(traceback.format_exc()) def _disconnect_socket(self): if self.connect_thread is not None: diff --git a/src/shufflelog_api.py b/src/shufflelog_api.py index 3c19750..4df890e 100644 --- a/src/shufflelog_api.py +++ b/src/shufflelog_api.py @@ -1,6 +1,22 @@ from itertools import product from messenger import * +""" +[debug] sending environment data to ShuffleLog +Traceback (most recent call last): + File "/home/ultraviolet/github/TagTracker/src/messenger.py", line 399, in read_messages + self._read_message() + File "/home/ultraviolet/github/TagTracker/src/messenger.py", line 532, in _read_message + handler.handle(message_type, message_data) + File "/home/ultraviolet/github/TagTracker/src/messenger.py", line 332, in handle + self.handler(type, MessageReader(data)) + File "/home/ultraviolet/github/TagTracker/src/shufflelog_api.py", line 21, in + self.msg.add_handler(ShuffleLogAPI._MSG_QUERY_ENVIRONMENT, lambda t, r: self._on_query_environment(t, r)) + File "/home/ultraviolet/github/TagTracker/src/shufflelog_api.py", line 57, in _on_query_environment + _write_matrix(builder, tag['transform']) +KeyError: 'transform' +""" + def _write_matrix(builder, matrix): # Write as column major for col, row in product(range(4), range(4)): @@ -51,10 +67,10 @@ def _on_query_environment(self, type, reader): builder = self.msg.prepare(ShuffleLogAPI._MSG_ENVIRONMENT) builder.add_int(len(self.tag_infos)) - for tag in self.tag_infos: + for id, tag in self.tag_infos.items(): builder.add_double(tag['size']) - builder.add_int(tag['id']) - _write_matrix(builder, tag['transform']) + builder.add_int(id) + _write_matrix(builder, tag['pose']) builder.add_int(len(self.camera_infos)) for camera in self.camera_infos: From 836e6bdaadff626b08891f23a32885300a018269 Mon Sep 17 00:00:00 2001 From: Mason Vogt <91571536+mvog2501@users.noreply.github.com> Date: Thu, 29 Dec 2022 09:09:33 -0600 Subject: [PATCH 07/10] Change euler angle definitions to degrees (was radians) --- src/solver.py | 2 +- src/transform_matrix.py | 64 +++++++++++++++++------------------------ 2 files changed, 27 insertions(+), 39 deletions(-) diff --git a/src/solver.py b/src/solver.py index 3e8975b..7502fdc 100644 --- a/src/solver.py +++ b/src/solver.py @@ -99,7 +99,7 @@ def __init__(self, environment_dict): continue # If it make it here, it must be Pitch, Yaw, Roll - pitch = rotation['pitch'] + pitch = rotation['pitch'] - 90 # Makes the default be a tag that is posted on a wall yaw = rotation['yaw'] roll = rotation['roll'] rotation_matrix = euler_to_matrix(pitch, yaw, roll) diff --git a/src/transform_matrix.py b/src/transform_matrix.py index 92f67c4..a7c12d0 100644 --- a/src/transform_matrix.py +++ b/src/transform_matrix.py @@ -2,44 +2,32 @@ import math def euler_to_matrix(pitch, yaw, roll): - # x is pitch - # y is yaw - # z is roll - - # Do sin an cosine for all axis - s_pitch = math.sin(pitch) - c_pitch = math.cos(pitch) - - s_yaw = math.sin(yaw) - c_yaw = math.cos(yaw) - - s_roll = math.sin(roll) - c_roll = math.cos(roll) - - # Initial matrix to edit - x = np.array( - [[1, 0, 0, 0], - [0, c_pitch, -s_pitch, 0], - [0, s_pitch, c_pitch, 0], - [0, 0, 0, 1]] - ) - - y = np.array( - [[c_yaw, 0, s_yaw, 0], - [0, 1, 0, 0], - [-s_yaw, 0, c_yaw, 0], - [0, 0, 0, 1]] - ) - - z = np.array( - [[c_roll, -s_roll, 0, 0], - [s_roll, c_roll, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1]] - ) - - # Combine indevidual matricies - return np.dot(z, np.dot(y,x)) + x_rad = math.radians(pitch) + y_rad = math.radians(yaw) + z_rad = math.radians(roll) + + rot_z = np.identity(4) + + rot_z[0,0] = math.cos(z_rad) + rot_z[0,1] = -math.sin(z_rad) + rot_z[1,0] = math.sin(z_rad) + rot_z[1,1] = math.cos(z_rad) + + rot_x = np.identity(4) + + rot_x[1,1] = math.cos(x_rad) + rot_x[1,2] = -math.sin(x_rad) + rot_x[2,1] = math.sin(x_rad) + rot_x[2,2] = math.cos(x_rad) + + rot_y = np.identity(4) + + rot_y[0,0] = math.cos(y_rad) + rot_y[0,2] = math.sin(y_rad) + rot_y[2,0] = -math.sin(y_rad) + rot_y[2,2] = math.cos(y_rad) + + return np.dot(rot_y, np.dot(rot_x, rot_z)) def apply_translation(matrix, x, y, z): matrix[0][3] = x From d95a27962a4d8ab9fee7fa5e0f7568c49bb4bab3 Mon Sep 17 00:00:00 2001 From: Mason Vogt <91571536+mvog2501@users.noreply.github.com> Date: Thu, 29 Dec 2022 09:11:13 -0600 Subject: [PATCH 08/10] Increase decimate for greater performance --- detector.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/detector.json b/detector.json index 449eaa6..cd7f7cf 100644 --- a/detector.json +++ b/detector.json @@ -2,7 +2,7 @@ "families" : "tag16h5", "border" : 1, "nthreads" : 4, - "quad_decimate" : 1, + "quad_decimate" : 4, "quad_blur" : 0, "quad_sigma" : 1, "refine_edges" : true, From 3b0e79b1fb649eae1429f3ef160248d157c683be Mon Sep 17 00:00:00 2001 From: Mason Vogt <91571536+mvog2501@users.noreply.github.com> Date: Thu, 29 Dec 2022 09:11:57 -0600 Subject: [PATCH 09/10] Use new degrees for euler angles --- environment.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/environment.json b/environment.json index cd9ea1a..0de7800 100644 --- a/environment.json +++ b/environment.json @@ -13,7 +13,7 @@ }, "rotation": { - "pitch" : 3.1415, + "pitch" : 0, "yaw": 0, "roll": 0 } From 71f9bafc96154111ef00d50f8f6747a83069fb3d Mon Sep 17 00:00:00 2001 From: Mason Vogt <91571536+mvog2501@users.noreply.github.com> Date: Thu, 29 Dec 2022 09:12:30 -0600 Subject: [PATCH 10/10] Stop flipping camera rotation --- cameras.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cameras.json b/cameras.json index ee3191e..26573f3 100644 --- a/cameras.json +++ b/cameras.json @@ -7,10 +7,10 @@ "port": 0, "robot_pose": [ [1, 0, 0, 0], - [0, -1, 0, 0], - [0, 0, -1, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], [0, 0, 0, 1] ] } ] -} \ No newline at end of file +}