add new function to output list of AT info

Code/Control/Laptop_Code/ESP32_AT/imageTread_AT_multiple.py

@@ -5,6 +5,15 @@ import time
 import math
 # import apriltag
 from pupil_apriltags import Detector
+
+detector = Detector(families='tag36h11',
+                    nthreads=1,
+                    quad_decimate=1.0,
+                    quad_sigma=0.0,
+                    refine_edges=1,
+                    decode_sharpening=0.25,
+                    debug=0)
+
 def nothing(x):
     pass
 
@@ -83,7 +92,6 @@ def get_AT_6DOF_info(url):
     # show the output image after AprilTag detection
     cv2.imshow("Image", frame)
 
-
     return Every_April_Tag_Infos
 
 
@@ -95,38 +103,71 @@ def rotationMatrixToEulerAngles(rx,ry,rz) :
     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.0.176/cam-hi.jpg' # 2
-    url = 'http://192.168.0.112/cam-hi.jpg' # 4
-    url = "http://192.168.1.8/cam-hi.jpg"
 
+def AT_info_list(Every_April_Tag_Infos):
+    num_of_ATs = len(Every_April_Tag_Infos)
 
+    tids = []
+    txs,tys,tzs = [],[],[]
+    rxs,rys,rzs = [],[],[]
 
-    # cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
+    if num_of_ATs != 0:
+        for i in range(num_of_ATs):
+            if i == 0:
+                print("========================")
+                print("=== Num of AT: {} =======".format(num_of_ATs))
+                print("========================")
 
-    # cv2.namedWindow("live transmission", cv2.WINDOW_NORMAL)
+            tid = Every_April_Tag_Infos[i][0]
+            tx = Every_April_Tag_Infos[i][1]
+            ty = Every_April_Tag_Infos[i][2]
+            tz = Every_April_Tag_Infos[i][3]
+            rx = Every_April_Tag_Infos[i][4]
+            ry = Every_April_Tag_Infos[i][5]
+            rz = Every_April_Tag_Infos[i][6]
+
+            tids.append(tid)
+            txs.append(tx)
+            tys.append(ty)
+            tzs.append(tz)
+            rxs.append(rx)
+            rys.append(ry)
+            rzs.append(rz)
+    return tids,txs,tys,tzs,rxs,rys,rzs
+
+def get_AT_6DOF_info_list(url):
+    Every_April_Tag_Infos = get_AT_6DOF_info(url)
+    ids,txs,tys,tzs,rxs,rys,rzs = AT_info_list(Every_April_Tag_Infos)
+    return ids,txs,tys,tzs,rxs,rys,rzs
 
-    detector = Detector(families='tag36h11',
-                       nthreads=1,
-                       quad_decimate=1.0,
-                       quad_sigma=0.0,
-                       refine_edges=1,
-                       decode_sharpening=0.25,
-                       debug=0)
+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.0.176/cam-hi.jpg' # 2
+    # url = 'http://192.168.0.112/cam-hi.jpg' # 4
+    url = "http://10.0.0.3/cam-hi.jpg"
 
     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:
-        Every_April_Tag_Infos = get_AT_6DOF_info(url)
-
+        # Every_April_Tag_Infos = get_AT_6DOF_info(url)
+        # ids,txs,tys,tzs,rxs,rys,rzs = AT_info_list(Every_April_Tag_Infos)
+
+        ids, txs, tys, tzs, rxs, rys, rzs = get_AT_6DOF_info_list(url)
+        print("ids:{}".format(ids))
+        print("txs:{}".format(txs))
+        print("tys:{}".format(tys))
+        print("tzs:{}".format(tzs))
+        print("rxs:{}".format(rxs))
+        print("rys:{}".format(rys))
+        print("rzs:{}".format(rzs))
+        """
         num_of_ATs = len(Every_April_Tag_Infos)
 
         for i in range(num_of_ATs):
@@ -154,9 +195,11 @@ if __name__ == "__main__":
             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()
+    cv2.destroyAllWindows()
+
+