Merge remote-tracking branch 'origin/main' into main

Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py

@@ -2,7 +2,7 @@ import cv2
 from urllib.request import urlopen, Request
 import numpy as np
 import time
-
+import math
 # import apriltag
 from pupil_apriltags import Detector
 def nothing(x):
@@ -11,16 +11,23 @@ def nothing(x):
 
 # detector = apriltag.Detector()
 
-detector = Detector()
+
 
 def get_AT_6DOF_info(url):
+    tid,tx,ty,tz= 0,0,0,0
+    rx = [0.0,0.0,0.0]
+    ry = [0.0, 0.0, 0.0]
+    rz = [0.0, 0.0, 0.0]
     header = {
         "User-Agent": "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/92.0.4515.159 Safari/537.36."}
     req = Request(url, headers=header)
     img_resp = urlopen(req, timeout=60)
     imgnp = np.array(bytearray(img_resp.read()), dtype=np.uint8)
     frame = cv2.imdecode(imgnp, -1)
+
+    # ret,frame = cap.read()
     gray_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
     h, w, _ = frame.shape
     # put a dot in center of the frame
     cv2.circle(frame, (w // 2, h // 2), 7, (255, 0, 0), -1)
@@ -29,7 +36,8 @@ def get_AT_6DOF_info(url):
     fy = 600
     cx = 400
     cy = 300
-    results = detector.detect(gray_image, estimate_tag_pose=True, camera_params=[fx, fy, cx, cy], tag_size=0.16)
+    AT_size = 0.16
+    results = detector.detect(gray_image, estimate_tag_pose=True, camera_params=[fx, fy, cx, cy], tag_size=AT_size)
     debug_print = 0
     for r in results:
         # extract the bounding box (x, y)-coordinates for the AprilTag
@@ -41,6 +49,7 @@ def get_AT_6DOF_info(url):
         ptA = (int(ptA[0]), int(ptA[1]))
         tx, ty, tz = r.pose_t # in meters
         rx,ry,rz = r.pose_R
+        tid = r.tag_id
 
         # draw the bounding box of the AprilTag detection
         cv2.line(frame, ptA, ptB, (0, 255, 0), 5)
@@ -55,7 +64,8 @@ def get_AT_6DOF_info(url):
 
 
         tagFamily = r.tag_family.decode("utf-8")
-        tid = r.tag_id
+        
+        
         cv2.putText(frame, tagFamily, (ptA[0], ptA[1] - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
 
         cv2.putText(frame, "tx: {:.2f}  ty: {:.2f}  tz:{:.2f}".format(tx[0], ty[0], tz[0]), (ptA[0], ptA[1] + 30),
@@ -71,20 +81,35 @@ def get_AT_6DOF_info(url):
 
     return tid,tx,ty,tz,rx,ry,rz
 
+
+def rotationMatrixToEulerAngles(rx,ry,rz) :
+
+    sy = math.sqrt(rx[0] * rx[0] + ry[0] * ry[0])
+    roll = math.atan2(rz[1] , rz[2])
+    pitch = math.atan2(-rz[0], sy)
+    yaw = math.atan2(ry[0], rx[0])
+
+    return np.array([roll, pitch, yaw])
 if __name__ == "__main__":
     #change the IP address below according to the
     #IP shown in the Serial monitor of Arduino code
     # url='http://192.168.4.1/cam-hi.jpg'
     # url='http://192.168.1.107/cam-hi.jpg'
     url='http://192.168.4.1/cam-hi.jpg'
-    # url = 'http://192.168.1.118/cam-hi.jpg'
+    url = 'http://192.168.1.118/cam-hi.jpg'
 
 
     # cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
 
     # cv2.namedWindow("live transmission", cv2.WINDOW_NORMAL)
 
+    detector = Detector()
+    tid,tx,ty,tz,rx,ry,rz = 0,0,0,0,0,0,0
+    test_webcam = 0
+    if test_webcam == 1:
+        cap = cv2.VideoCapture(0)
     while True:
+        """
         header = {"User-Agent": "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/92.0.4515.159 Safari/537.36."}
         req = Request(url, headers=header)
         img_resp = urlopen(req, timeout=60)
@@ -97,7 +122,7 @@ if __name__ == "__main__":
 
         # put a dot in center of the frame
         cv2.circle(frame, (w//2, h//2), 7, (255, 0, 0), -1)
-
+        """
         """
         If you also want to extract the tag pose, estimate_tag_pose should be set to True
         and camera_params ([fx, fy, cx, cy])
@@ -117,6 +142,7 @@ if __name__ == "__main__":
         fx, fy, cx, cy are given in Pixels in Computer Vision ( and openCV)
         but e.g. in Photogrammetry you often use mm
         """
+        """
         fx = 800
         fy = 600
         cx = 400
@@ -156,11 +182,24 @@ if __name__ == "__main__":
 
         # show the output image after AprilTag detection
         cv2.imshow("Image", frame)
+        """
+        tid,tx,ty,tz,rx,ry,rz = get_AT_6DOF_info(url)
 
+        print("testing new function")
+        print("-----------------------")
+        print("tid:{}".format(tid))
+        print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+        print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+
+        # R = np.array([rx,ry,rz])
+        roll,pitch, yaw = rotationMatrixToEulerAngles(rx,ry,rz)
+        roll = roll * 180 /math.pi
+        pitch = pitch * 180 / math.pi
+        yaw = yaw * 180 / math.pi
+        print("roll,pitch,yaw:{},{},{}".format(roll, pitch, yaw))
 
         key=cv2.waitKey(5)
         if key==ord('q'):
             break
 
-
     #cv2.destroyAllWindows()

Code/Control/Laptop_Code/README.md

-[Progress] The overall logic is clear and tested! 
+# Laptop Code
 
-What to do next in python main:
+## 1 Main structure 
+
+## 2 Software Requirements
+
+### Step 1: 
+
+Download Anaconda or Pycharm
+
+### Step 2: 
+
+- If using Anaconda, then create a virtual environment first:
+
+```shell
+# crreate virtual environment
+conda create -n FORAYenv python=3.7
+# activate virtual environment
+conda activate FORAYenv
+# install environment installation tool pip (if it doesn't have one)
+conda install pip
+```
+
+- If using Pycharm, at the very beginning, create a virtual environment
+
+![](\Figs\fig1.png)
+
+### Step 3:
+
+Install dependency
+
+- If using Anaconda, install it from the terminal. And always remember to activate the conda environment fist: `conda activate FORAYenv`
+
+```shell
+# install pyserial
+pip install pyserial
+# install pygame 
+pip install pygame
+# install cv2
+pip install opencv-python
+# install 
+```
 
-- Have keyboard interruption in the python function
-  -  To tweak PID value while running the program
-  - To stop the balloon from being crazy 
-- The meter version April tag detection is still problematic so we need to tune that 
-- Use field test experiment to test PID
-- 
\ No newline at end of file

Code/Control/Laptop_Code/constants.py

@@ -46,3 +46,7 @@ kpy_g,kiy_g,kdy_g = 1.2, 0.01, 0.5
 
 # difference between center of AT and center of goal
 AT_goal_Delta = 110 #cm
+
+AT_h1 = 50 # meters
+
+kph,kih,kdh = 1.2,0.01,0.5
\ No newline at end of file

Code/Control/Laptop_Code/main_keyboard.py

 import time
 import serial
-import ball_detection.ball_detection as ball_detection
+
 import simple_pid.PID as PID
 import timeit
 import pygame
 from constants import *
+from ESP32_AT.imageTread_AT import get_AT_6DOF_info
 
 global ml
-ml = 1
+ml = 0
+if ml == 1:
+    import ball_detection.ball_detection as ball_detection
+
 # ========= Serial Port I/O ===========
 
 def serial_port_in(serial_port):
@@ -118,6 +122,12 @@ def ball_capture(LIDAR_dist):
     else:
         return False
 
+def AT_detect(tx,ty):
+    if tx == 0 and ty == 0:
+        return False
+    else:
+        return True
+
 def stop_all():
     pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
     dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
@@ -197,17 +207,65 @@ def seeking():
         dir1, dir2, dir3, dir4
     """
 
-    pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+    delta_h = 2
+    url = 'http://192.168.1.118/cam-hi.jpg'
+    tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+
+    AT_detected = AT_detect(tx, ty)
+    count_h = 0
+
+    threshold_h = 0.3
+    if AT_detected:
+        current_h = get_altitude_from_AT(AT_h1,ty)
+        count_h += 1
+        if count_h == 1:
+            set_h = current_h + delta_h  # in meter
+        pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+        diff_h = abs(set_h - current_h)
+        if diff_h < threshold_h:
+            new_ssp = 200
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction_ssp(new_ssp)
+        else:
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+    else:
+        pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
 
 
     return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
 
+def vertical_control(current_h,set_h):
+
+    pid_h = PID(kph, kih, kdh, setpoint = set_h)
+    pid_h.auto_mode = True
+    pid_h.set_auto_mode(True, last_output = 8.0)
+    pid_h.output_limits = (-255,255)
+
+    input_h = current_h
+    output_h = pid_h(input_h)
+
+    pwm1 = abs(output_h)
+    pwm2 = abs(output_h)
+    pwm3 = 0
+    pwm4 = 0
+    dir1 = '+'
+    dir2 = '+'
+    dir3 = '+'
+    dir4 = '+'
+
+    return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
 def rotate_one_direction():
     pwm1, pwm2, pwm3, pwm4 = 0, 0, seeking_speed, seeking_speed
     dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
 
     return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
 
+def rotate_one_direction_ssp(ssp):
+    pwm1, pwm2, pwm3, pwm4 = 0, 0, ssp, ssp
+    dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
+
+    return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
 def move2ball(gbx, gby, gb_dist):
     """
     Description:
@@ -303,13 +361,13 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
             pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
         else:  # Goal not detected
             # stop_all()  # Debug
-            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
     else:  # Ball not captured
         if ballDetect:  # Ball detected
             pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
         else:  # Ball not detected
             # stop_all()  # Debug
-            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
 
     return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
 
@@ -480,6 +538,24 @@ def decode_ctl(Ctl_com):
     dir3 = Ctl_com[6]
     dir4 = Ctl_com[7]
     return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def get_altitude_from_AT(AT_h,ty):
+    # below AT center, ty > 0
+    # above AT center, ty < 0
+    altitude = AT_h - ty
+    return altitude
+
+
+def test_function():
+    url = 'http://192.168.1.118/cam-hi.jpg'
+    tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+    print("testing new function")
+    print("-----------------------")
+    print("tid:{}".format(tid))
+    print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+    print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+
+
 # ===== Main Function =====
 if __name__ == '__main__':
     # =========== SET UP ============
@@ -550,6 +626,12 @@ if __name__ == '__main__':
                 flag = 0
                 print_count = 1
 
+        elif get_key('t'):
+            flag = 4
+            while (flag == 4):
+                test_function()
+                flag, print_count = keyboard_stop(flag, print_count)
+
         elif get_key('k'):
             break