diff --git a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT_multiple.py b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT_multiple.py
index e3d10bd201fbdee64f26a7a1d5d36585c8d353d0..051e15706a666ab5e0ff014303eb3d83678ed596 100644
--- a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT_multiple.py
+++ b/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()
+
+