diff --git a/cameras.json b/cameras.json index fa525cc..26573f3 100644 --- a/cameras.json +++ b/cameras.json @@ -4,13 +4,13 @@ { "name": "USB Webcam", "type": "Small_USB_Camera", - "port": 2, + "port": 0, "robot_pose": [ - [1, 0, 0, 0.5], - [0, -1, 0, 0], - [0, 0, -1, 0], + [1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], [0, 0, 0, 1] ] } ] -} \ No newline at end of file +} 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, diff --git a/environment.json b/environment.json index c3f35d8..0de7800 100644 --- a/environment.json +++ b/environment.json @@ -1,27 +1,24 @@ { - "tag_family" : "tag16h5", + "tag_family" : "36h11", "tags" : [ - { - "size": 0.173, - "id": 1, - "transform": [ - [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] - ] + { + "size" : 0.175, + "id": 9, + "pose": { + "translation": { + "x": 3.594, + "y": 0, + "z": 0.2 + }, + + "rotation": { + "pitch" : 0, + "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/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: diff --git a/src/solver.py b/src/solver.py index a6c3b27..7502fdc 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 euler_to_matrix, apply_translation +from transformations import quaternion_matrix # TODO-Ryan: Finish/Fix @@ -57,7 +59,54 @@ 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 + + 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]['pose'] = matrix + continue + + translation = pose['translation'] + rotation = pose['rotation'] + + # See if it is a quaternion rotation + quaternion = rotation.get('quaternion') + if quaternion is not None: + 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]['pose'] = matrix + continue + + # If it make it here, it must be Pitch, Yaw, Roll + 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) + matrix = apply_translation(rotation_matrix, translation['x'], translation['y'], translation['z']) + self.tags_dict[tag_id]['pose'] = matrix + + self.tag_family = environment_dict['tag_family'] if self.tag_family != "tag16h5": @@ -83,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) diff --git a/src/transform_matrix.py b/src/transform_matrix.py new file mode 100644 index 0000000..a7c12d0 --- /dev/null +++ b/src/transform_matrix.py @@ -0,0 +1,37 @@ +import numpy as np +import math + +def euler_to_matrix(pitch, yaw, roll): + 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 + matrix[1][3] = y + matrix[2][3] = z + return matrix +