More ways to define tag positions

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]
             ]
         }
     ]
-}
+}

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,

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
+                    }
+
+                }
         }
     ]
 }

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

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

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:

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 <lambda>
+    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:

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)

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
+