Dev/tagwork

README.md

@@ -1 +1,17 @@
-# robomaster_cv_py
+# UT RoboMaster CV - Stampede (Tag Detection Branch)
+
+This branch builds on Eddie's tag detection python code. The goals are as follows:
+1. Reliably and accurately detect only the DJI tags given a video feed
+2. Reliably extract a translation vector and relative position from the detected DJI tag
+3. Reliably obtain a field position and heading for the camera given the absolute tag positions.
+
+## Important Notes (Aditya Pulipaka, adipu@utexas.edu, Discord: adipu24)
+
+Currently, I have been trying to perfect this python code. It seems that it came from the official RoboMaster CV repository, but a version written in Python. While there are many components and subfolders within this repository that include armor plate detection and ranging, the main focus of this branch is the tag_detector subfolder and the calibration code. These are the important elements for tag-based localization.
+
+**Function**
+'newDetector.py' will detect multiple red and/or DJI tags at once and print their colors, letters, and relative positions.
+
+**Calibration**
+
+The camera matrix in tag_detector.py and newDetector.py is for my 2022 macbook air. If recalibration is desired, use 'basicCalib.py' with photos taken on your camera placed in the 'laptop calibration' directory instead of what's there currently. Change the *inner dimensions* of the chessboard at the beginning of 'basicCalib.py' if needed.

SettingUp_Repo.md

@@ -0,0 +1,20 @@
+Written by Paul J. Han (Discord bornhater#7417)
+Last modified: 2/11/2023 
+All of these commands were done in Powershell, but theoretically should work on any other shell (i.e Git Bash, etc.)
+If you have any questions or suggestions, feel free to message Hasif or Wenyu on Discord (Has02#1313 and Wenyu#3334), the CV leads for the UT Robomaster team.
+Otherwise, feel free to contact me on Discord as well.
+
+1. Go to the folder that you wish the repo to exist in using commands such as **cd** 
+
+2. Once you are in the desired folder, type in **git clone https://github.com/ut-ras/robomaster_CV.git**
+This step will allow you to clone the repo and all of its contents
+
+3. Once you have cloned the repo, enter the folder of the repo in the terminal using the command **cd robomaster_CV**
+
+4. Once you have entered the repo, we want to enter the branch of the subteam that you want to work on. To do this, we will enter in this command.
+**git checkout -t origin/feature/subteam-name**. The **subteam-name** will be replaced by the actual name of the subteam that you wish to work on. For example,
+this command would be ***git checkout -t origin/feature/depth_calculation*** if you wished to checkout the branch of the depth calculation subteam. 
+
+What this command does is it creates a local branch with the same name as the remote branch and assigns the local branch to track the contents of the remote branch, allowing you to push and pull from the remote server and retrieve updates easily. 
+
+For example, if I did ***git checkout -t origin/feature/subteam-name***, it would create a local branch called **feature/subteam-name** and then set the remote upstream to be **remotes/origin/feature/subteam-name**.

basicCalib.py

@@ -0,0 +1,98 @@
+# Import required modules 
+import cv2 
+import numpy as np 
+import os 
+import glob 
+
+
+# Define the dimensions of checkerboard 
+CHECKERBOARD = (6, 9) 
+
+
+# stop the iteration when specified 
+# accuracy, epsilon, is reached or 
+# specified number of iterations are completed. 
+criteria = (cv2.TERM_CRITERIA_EPS +
+			cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 
+
+
+# Vector for 3D points 
+threedpoints = [] 
+
+# Vector for 2D points 
+twodpoints = [] 
+
+
+# 3D points real world coordinates 
+objectp3d = np.zeros((1, CHECKERBOARD[0] 
+					* CHECKERBOARD[1], 
+					3), np.float32) 
+objectp3d[0, :, :2] = np.mgrid[0:CHECKERBOARD[0], 
+							0:CHECKERBOARD[1]].T.reshape(-1, 2) 
+prev_img_shape = None
+
+
+# Extracting path of individual image stored 
+# in a given directory. Since no path is 
+# specified, it will take current directory 
+# jpg files alone 
+images = glob.glob('realsense calibration/*.jpg')
+for filename in images: 
+	image = cv2.imread(filename) 
+	grayColor = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) 
+
+	# Find the chess board corners 
+	# If desired number of corners are 
+	# found in the image then ret = true 
+	ret, corners = cv2.findChessboardCorners( 
+					grayColor, CHECKERBOARD, 
+					cv2.CALIB_CB_ADAPTIVE_THRESH 
+					+ cv2.CALIB_CB_FAST_CHECK +
+					cv2.CALIB_CB_NORMALIZE_IMAGE) 
+
+	# If desired number of corners can be detected then, 
+	# refine the pixel coordinates and display 
+	# them on the images of checker board 
+	if ret == True: 
+		threedpoints.append(objectp3d) 
+
+		# Refining pixel coordinates 
+		# for given 2d points. 
+		corners2 = cv2.cornerSubPix( 
+			grayColor, corners, (11, 11), (-1, -1), criteria) 
+
+		twodpoints.append(corners2) 
+
+		# Draw and display the corners 
+		image = cv2.drawChessboardCorners(image, 
+										CHECKERBOARD, 
+										corners2, ret) 
+
+	cv2.imshow('img', image) 
+	cv2.waitKey(0) 
+
+cv2.destroyAllWindows() 
+
+h, w = image.shape[:2] 
+
+
+# Perform camera calibration by 
+# passing the value of above found out 3D points (threedpoints) 
+# and its corresponding pixel coordinates of the 
+# detected corners (twodpoints) 
+ret, matrix, distortion, r_vecs, t_vecs = cv2.calibrateCamera( 
+	threedpoints, twodpoints, grayColor.shape[::-1], None, None) 
+
+
+# Displaying required output 
+print(" Camera matrix:") 
+print(matrix) 
+
+print("\n Distortion coefficient:") 
+print(distortion) 
+
+print("\n Rotation Vectors:") 
+print(r_vecs) 
+
+print("\n Translation Vectors:") 
+print(t_vecs) 

bounding_box/__init__.py

@@ -0,0 +1 @@
+from bounding_box import *

bounding_box/bounding_box.py

@@ -0,0 +1,92 @@
+import time
+import numpy as np
+class BoundingBox:
+	def __init__(self):
+		self.__x1__ = 0
+		self.__x2__ = 0
+		self.__y1__ = 0
+		self.__y2__ = 0
+		self.__x_center__ = 0
+		self.__y_center__ = 0
+		self.__depth_value__ = 0
+		self.__x_coord__ = 0	# In meters
+		self.__y_coord__ = 0	# In meters
+		self.__z_coord__ = 0	# In meters
+		self.__height__ = 0
+		self.__width__ = 0
+		self.__time__ = 0
+
+	def set_x_value(self,x1, x2):
+		self.__x1__ = x1
+		self.__x2__ = x2
+
+	def set_y_value(self,y1, y2):
+		self.__y1__ = y1
+		self.__y2__ = y2
+
+	def calculate_x_center(self):
+		self.__x_center__ = np.float32((self.__x1__ + self.__x2__)/2)
+
+	def calculate_y_center(self):
+		self.__y_center__ = np.float32((self.__y1__ + self.__y2__)/2)
+
+	def set_depth(self, depth):
+		self.__depth_value__ = depth
+
+	def set_x_coord(self, x_coord):
+		self.__x_coord__ = x_coord
+
+	def set_y_coord(self, y_coord):
+		self.__y_coord__ = y_coord
+
+	def set_z_coord(self, z_coord):
+		self.__z_coord__ = z_coord
+
+	def calculate_height(self):
+		self.__height__ = self.__y2__ - self.__y1__
+
+	def calculate_width(self):
+		self.__width__ = self.__x2__ - self.__x1__
+
+	def set_time(self):
+		self.__time__ = time.localtime(time.time())
+
+	def get_x_value(self):
+		return self.__x1__, self.__x2__
+
+	def get_y_value(self):
+		return self.__y1__, self.__y2__
+
+	def get_x_center(self):
+		self.calculate_x_center()
+		return self.__x_center__
+
+	def get_y_center(self):
+		self.calculate_y_center()
+		return self.__y_center__
+
+	def get_depth(self):
+		return self.__depth_value__
+
+	def get_x_coord(self):
+		return self.__x_coord__
+
+	def get_y_coord(self):
+		return self.__y_coord__
+
+	def get_z_coord(self):
+		return self.__z_coord__
+
+	def get_height(self):
+		return self.__height__
+
+	def get_width(self):
+		return self.__width__
+
+	def get_time(self):
+		return self.__time__
+
+	def print_time(self):
+		print(self.__time__)
+
+

communication/__init__.py

@@ -0,0 +1 @@
+from communication import *

communication/beaglebone.md

@@ -0,0 +1,60 @@
+The Beaglebone should have UART ports available. Port 0 is activated by default, but is being used as the kernel debug log so a lot of information is dumped here into terminal. 
+
+**Here is how to activate a UART port.**
+
+https://forum.beagleboard.org/t/need-to-enable-7-uart-on-beaglebone-ai-64/33936/2
+
+What is an overlay and dtsi file?
+
+https://forum.beagleboard.org/t/enable-uart-1-on-bb-ai-64/32997/8
+
+May have some important information.
+
+**Interfacing with a UART port**
+
+In order to interface with UART ports, we will be using the **serial** library from Python.
+
+1. In order to use this library, first run this command in command line.
+
+	**pip install pyserial**
+
+2. There should be some scripts already written in the **communication** branch of our Github repository. Please read the code and available comments in order to familiarize yourself with this library.
+
+https://pyserial.readthedocs.io/en/latest/pyserial.html
+
+In order to read data, we may use a logic analyzer in order to connect jumper wires to the pins and read UART information from there.
+
+Currently attempting to grant the Beaglebone Internet access.
+
+**Setting the System Clock on the Beaglebone**
+
+As the Beaglebone does not have a battery, it cannot keep track of the clock when turned off and defaults to whatever it was on last. In order to access the Internet and pass the HTTPS security protocols, we must set the system clock to be synced when we turn on the Beaglebone.
+
+To check the time for correctness, run **date** in the terminal.
+
+http://derekmolloy.ie/automatically-setting-the-beaglebone-black-time-using-ntp/
+
+This guide should help
+
+https://askubuntu.com/questions/1058593/how-to-sync-the-time-to-network-with-timedatectl-on-ubuntu-18-04
+
+This one as well
+
+**Testing**
+
+Both the read and write scripts are written.
+
+To test, two terminals can be opened. One terminal runs the read script and the other terminal runs the write script.
+The result of the transmission can be printed onto the terminal. If this doesn't work, using a logic analyzer on the pins could be a valid strategy. 
+
+**Pins**
+
+https://docs.beagleboard.org/latest/boards/beaglebone/ai-64/ch07.html#p8-17-p8-19
+
+
+**UART Pins**
+
+In order to determine which pins are mapped to the symbolic link that can be found by running *tree /dev/bone*, we must access the dtsi file and configure the pins ourselves.
+
+For whatever reason, the symbolic link for UART1 is actually connected to UART2's pins.
+

communication/communication.py

@@ -0,0 +1,82 @@
+import serial
+import struct
+import crc
+import sys
+from enum import IntEnum
+
+"""
+Communication.py is aimed to send information to the beaglebone. The way of doing so is to utilize UART
+"""
+
+# Serial communication
+ser = serial.Serial()
+
+"""
+Initialize_communication initializes the communication port. Tentatively, the COM port is /dev/bone/ttyS4 with a baud rate of 115200 and a timeout of 0.5
+"""
+def initialize_communication():
+    ser.port = '/dev/ttyS4'
+    ser.baudrate = 115200
+    ser.timeout = 0.5
+    ser.open()
+
+
+def read_message():
+    message = ser.readline()
+    return message
+
+def send_message(data, data_format, message_type):
+    frame_head_byte = 0xA5
+    frame_data_length = data_format.size
+    frame_sequence_number = 0
+
+    frame_header = FrameHeaderFormat.pack(frame_head_byte, frame_data_length, frame_sequence_number)
+    frame_crc = crc8.checksum(frame_header)
+
+    MessageNoCRC16Format = struct.Struct('<{}sBH{}s'.format(FrameHeaderFormat.size, frame_data_length))
+    message_no_crc16 = MessageNoCRC16Format.pack(frame_header, frame_crc, message_type.value, data)
+    message_crc = crc16.checksum(message_no_crc16)
+
+    FullMessageFormat = struct.Struct('<{}sH'.format(MessageNoCRC16Format.size))
+    full_message = FullMessageFormat.pack(message_no_crc16, message_crc)
+
+    ser.write(full_message)
+
+# https://docs.python.org/3/library/struct.html#format-strings
+TurretDataFormat = struct.Struct("<fffffffff?")
+FrameHeaderFormat = struct.Struct("<BHB")
+
+crc8config = crc.Configuration(
+    width=8,
+    polynomial=0x31,
+    init_value=0xFF,
+    final_xor_value=0x00,
+    reverse_input=True,
+    reverse_output=True,
+)
+
+crc16config = crc.Configuration(
+    width=16,
+    polynomial=0x1021,
+    init_value=0xFFFF,
+    final_xor_value=0x0000,
+    reverse_input=True,
+    reverse_output=True,
+)
+
+crc8 = crc.Calculator(crc8config)
+
+crc16 = crc.Calculator(crc16config)
+
+
+# Reading message
+class MessageType(IntEnum):
+    CMD_Odometry_Data = 1
+    CMD_Turret_Aim = 2
+
+# Sending message
+def send_turret_data(xPos, yPos, zPos, 
+                  xVel, yVel, zVel, 
+                  xAcc, yAcc, zAcc, 
+                  hasTarget):
+    send_message(TurretDataFormat.pack(xPos, yPos, zPos, xVel, yVel, zVel, xAcc, yAcc, zAcc, hasTarget), TurretDataFormat, MessageType.CMD_Turret_Aim)

depth_calculation/__init__.py

@@ -0,0 +1 @@
+from depth_calculation import *